My collection of python snippets.
- bare_plot.py: Publish a marker message.
Last active
January 26, 2021 15:50
-
-
Save mpkuse/690376f51e14499c24bf7b6db28f8f6a to your computer and use it in GitHub Desktop.
ROS Python Snippets
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| import rospy | |
| from std_msgs.msg import String | |
| from visualization_msgs.msg import Marker | |
| def make_marker(): | |
| marker = Marker() | |
| marker.header.frame_id = "world" | |
| marker.header.stamp = rospy.Time.now() | |
| marker.ns = "my_namespace" | |
| marker.id = 0 | |
| marker.type = marker.SPHERE | |
| marker.action = marker.ADD | |
| marker.pose.position.x = 1 | |
| marker.pose.position.y = 1 | |
| marker.pose.position.z = 1 | |
| marker.pose.orientation.x = 0.0 | |
| marker.pose.orientation.y = 0.0 | |
| marker.pose.orientation.z = 0.0 | |
| marker.pose.orientation.w = 1.0 | |
| marker.scale.x = 1 | |
| marker.scale.y = 11 | |
| marker.scale.z = 11 | |
| marker.color.a = 1.0 | |
| marker.color.r = 0.0 | |
| marker.color.g = 1.0 | |
| marker.color.b = 0.0 | |
| return marker | |
| def publish_marker( pub ): | |
| marker = make_marker() | |
| pub.publish( marker ) | |
| if __name__ == '__main__': | |
| rospy.init_node('talker', anonymous=True) | |
| # pub = rospy.Publisher('chatter', String, queue_size=10) | |
| pub = rospy.Publisher('chatter', Marker, queue_size=10) | |
| rate = rospy.Rate(10) # 10hz | |
| while not rospy.is_shutdown(): | |
| publish_marker( pub ) | |
| rate.sleep() |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| def _init_camera_marker( self ): | |
| marker = Marker() | |
| marker.action = marker.ADD | |
| marker.type = marker.LINE_LIST | |
| cam_size = 2.0 | |
| __vcam_width = 0.7*cam_size; | |
| __vcam_height = 0.4*cam_size; | |
| __z = 1.0*cam_size; | |
| marker.points = [] | |
| pt = Point() | |
| pt.x = 0; pt.y=0; pt.z=0; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = 0; pt.y=0; pt.z=0; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = 0; pt.y=0; pt.z=0; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = 0; pt.y=0; pt.z=0; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = -__vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=-__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| pt.x = __vcam_width; pt.y=__vcam_height; pt.z=__z; | |
| pt.x, pt.y, pt.z = pt.z, -pt.y, pt.x | |
| marker.points.append( copy.deepcopy( pt ) ) | |
| # toset | |
| marker.pose.position.x = 0.; | |
| marker.pose.position.y = 0.; | |
| marker.pose.position.z = 0.; | |
| marker.pose.orientation.x = 0.; | |
| marker.pose.orientation.y = 0.; | |
| marker.pose.orientation.z = 0.; | |
| marker.pose.orientation.w = 1.; | |
| marker.color.r = 0.2;marker.color.b = 0.;marker.color.g = 0.;marker.color.a = 1. | |
| marker.scale.x = 0.3; #width of line-segments | |
| return marker | |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import rospy | |
| import rospkg | |
| import tf2_ros | |
| class myClass: | |
| def __init__(self) | |
| self.tf_buffer = tf2_ros.Buffer() | |
| self.tf_listner = tf2_ros.TransformListener( self.tf_buffer ) | |
| def get_current_tf( self ): | |
| """ returns PoseStamped of tf2 transform between /map to /baseframe | |
| The transform is /baseframe observed from /map frame. | |
| Returns: PoseStamped | |
| """ | |
| # TF_listner Lookup between /map /base_frame | |
| # REFERENCE: http://docs.ros.org/en/indigo/api/tf2_ros/html/c++/classtf2__ros_1_1Buffer.html | |
| map_frame_name = "map" | |
| robot_baseframe_name = "base_frame" | |
| try: | |
| # get latest transform aka `geometry_msgs/TransformStamped` | |
| trans = self.tf_buffer.lookup_transform(map_frame_name, robot_baseframe_name, rospy.Time(0)) | |
| print( '[rosUtil/get_current_tf] tf2 transform between ', map_frame_name , ' and ', robot_baseframe_name, ' --> ' ) | |
| print( '\ttranslation=%2.2f, %2.2f, %2.2f' %(trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z) ) | |
| print( '\torientation_xyzw=%2.2f, %2.2f, %2.2f, %2.2f' %(trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) ) | |
| # Convert this to `geometry_msgs::PoseStamped` | |
| curr_pose = PoseStamped() | |
| curr_pose.header = trans.header | |
| curr_pose.pose.position.x = trans.transform.translation.x | |
| curr_pose.pose.position.y = trans.transform.translation.y | |
| curr_pose.pose.position.z = trans.transform.translation.z | |
| curr_pose.pose.orientation.w = trans.transform.rotation.w | |
| curr_pose.pose.orientation.z = trans.transform.rotation.z | |
| curr_pose.pose.orientation.x = 0.0 | |
| curr_pose.pose.orientation.y = 0.0 # zero because we want a planar pose | |
| return curr_pose | |
| except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): | |
| print( 'TF listener failed') | |
| return None | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment