def publish_target_pose(self, position, orientation): self.target_pose_pub.publish( PoseStamped(header=mil_ros_tools.make_header('/map'), pose=Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion( orientation)))) self.distance_marker.header = mil_ros_tools.make_header('/map') self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\ 'Z: ' + str(self.target_depth) + 'm' self.distance_marker.pose = Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation)) self.target_distance_pub.publish(self.distance_marker)
def moveto_action(self, position, orientation): self.client.cancel_goal() rospy.logwarn("Going to waypoint") rospy.logwarn("Found server") # Stay under the water if position[2] > 0.3: rospy.logwarn("Waypoint set too high!") position[2] = -0.5 goal = mil_msgs.MoveToGoal( header=mil_ros_tools.make_header('/map'), posetwist=mil_msgs.PoseTwist( pose=geom_msgs.Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation) ) ), speed=0.2, linear_tolerance=0.1, angular_tolerance=0.1 ) self.client.send_goal(goal) # self.client.wait_for_result() rospy.logwarn("Go to waypoint")
def make_cylinder_marker(self, base, length, color, frame='/base_link', up_vector=np.array([0.0, 0.0, 1.0]), **kwargs): '''Handle the frustration that Rviz cylinders are designated by their center, not base''' center = base + (up_vector * (length / 2)) pose = Pose(position=mil_ros_tools.numpy_to_point(center), orientation=mil_ros_tools.numpy_to_quaternion( [0.0, 0.0, 0.0, 1.0])) marker = visualization_msgs.Marker( ns='sub', header=mil_ros_tools.make_header(frame=frame), type=visualization_msgs.Marker.CYLINDER, action=visualization_msgs.Marker.ADD, pose=pose, color=ColorRGBA(*color), scale=Vector3(0.2, 0.2, length), lifetime=rospy.Duration(), **kwargs) return marker
def _send_debug_marker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the object. This marker is a scaled cube with the demensions of the model. Only called if debug_ros param == True ''' if self.last3d is None or not self.found: return m = Marker() m.header.frame_id = '/map' m.header.stamp = self.last_found_time_3D m.ns = "orange_rectangle" m.id = 0 m.type = 1 m.action = 0 # Real demensions of path marker m.scale.x = self.rect_model.length m.scale.y = self.rect_model.width m.scale.z = 0.05 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(self.timeout_seconds) self.markerPub.publish(m)
def publish_target_pose(self, position, orientation): self.target_pose_pub.publish( PoseStamped( header=mil_ros_tools.make_header('/map'), pose=Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation) ) ) ) self.distance_marker.header = mil_ros_tools.make_header('/map') self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\ 'Z: ' + str(self.target_depth) + 'm' self.distance_marker.pose = Pose(position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation)) self.target_distance_pub.publish(self.distance_marker)
def moveto_action(self, position, orientation): self.client.cancel_goal() rospy.logwarn("Going to waypoint") rospy.logwarn("Found server") # Stay under the water if position[2] > 0.3: rospy.logwarn("Waypoint set too high!") position[2] = -0.5 goal = mil_msgs.MoveToGoal( header=mil_ros_tools.make_header('/map'), posetwist=mil_msgs.PoseTwist( pose=geom_msgs.Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation) ) ), speed=0.2, linear_tolerance=0.1, angular_tolerance=0.1 ) self.client.send_goal(goal) # self.client.wait_for_result() rospy.logwarn("Go to waypoint")
def _send_debug_marker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the object. This marker is a scaled cube with the demensions of the model. Only called if debug_ros param == True ''' if self.last3d is None or not self.found: return m = Marker() m.header.frame_id = '/map' m.header.stamp = self.last_found_time_3D m.ns = "orange_rectangle" m.id = 0 m.type = 1 m.action = 0 # Real demensions of path marker m.scale.x = self.rect_model.length m.scale.y = self.rect_model.width m.scale.z = 0.05 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(self.timeout_seconds) self.markerPub.publish(m)
def __init__(self): self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper assert self.layout is not None, 'Could not load thruster layout from ROS param' ''' Create MarkerArray with an arrow marker for each thruster at index node_id. The position of the marker is as specified in the layout, but the length of the arrow will be set at each thrust callback. ''' self.markers = MarkerArray() for i in xrange(len(self.layout)): # Initialize each marker (color, scale, namespace, frame) m = Marker() m.header.frame_id = '/base_link' m.type = Marker.ARROW m.ns = 'thrusters' m.color.a = 0.8 m.scale.x = 0.01 # Shaft diameter m.scale.y = 0.05 # Head diameter m.action = Marker.DELETE m.lifetime = rospy.Duration(0.1) self.markers.markers.append(m) for key, layout in self.layout.iteritems(): # Set position and id of marker from thruster layout idx = layout['node_id'] pt = numpy_to_point(layout['position']) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].points.append(pt) self.markers.markers[idx].id = idx # Create publisher for marker and subscribe to thrust self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5) self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def sendDebugMarker(self): ''' Sends a rviz marker in the camera frame with the estimated pose of the path marker. This marker is a scaled cube with the demensions and color of the actual marker. Only called if debug_ros param == True ''' m = Marker() m.header.frame_id = self.cam.tfFrame() m.header.stamp = self.last_found_time m.ns = "path_markers" m.id = 0 m.type = 1 m.action = 0 m.pose.position = numpy_to_point(self.last3d[0]) m.pose.orientation = numpy_to_quaternion(self.last3d[1]) # Real demensions of path marker m.scale.x = 1.2192 m.scale.y = 0.1524 m.scale.z = 0.05 m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.0 m.color.r = 1.0 m.color.a = 1.0 m.lifetime = rospy.Duration(0) self.markerPub.publish(m)
def cb_3d(self, req): res = VisionRequestResponse() if (self.last2d == None or not self.enabled): res.found = False else: res.pose.header.frame_id = self.cam.tfFrame() res.pose.header.stamp = self.last_found_time res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res
def get_thruster_info(self, srv): ''' Get the thruster info for a particular thruster name ''' query_name = srv.thruster_name info = self.ports[ self.thruster_to_port_map[query_name]].thruster_info[query_name] thruster_info = ThrusterInfoResponse( node_id=info.node_id, min_force=info.thrust_bounds[0], max_force=info.thrust_bounds[1], position=numpy_to_point(info.position), direction=Vector3(*info.direction)) return thruster_info
def get_thruster_info(self, srv): ''' Get the thruster info for a particular thruster name ''' query_name = srv.thruster_name info = self.ports[self.thruster_to_port_map[query_name]].thruster_info[query_name] thruster_info = ThrusterInfoResponse( node_id=info.node_id, min_force=info.thrust_bounds[0], max_force=info.thrust_bounds[1], position=numpy_to_point(info.position), direction=Vector3(*info.direction) ) return thruster_info
def _vision_cb_3D(self, req): res = VisionRequestResponse() if self.last_found_time_3D is None or self.image_sub.last_image_time is None: res.found = False return res dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() if dt < 0 or dt > self.timeout_seconds: res.found = False elif (self.last3d is None or not self.enabled): res.found = False else: res.pose.header.frame_id = "/map" res.pose.header.stamp = self.last_found_time_3D res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res
def draw_sphere(position, color, scaling=(0.11, 0.11, 0.11), m_id=4, frame='/base_link'): pose = Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0]) ) marker = visualization_msgs.Marker( ns='wamv', id=m_id, header=mil_ros_tools.make_header(frame=frame), type=visualization_msgs.Marker.SPHERE, action=visualization_msgs.Marker.ADD, pose=pose, color=ColorRGBA(*color), scale=Vector3(*scaling), lifetime=rospy.Duration(), ) rviz_pub.publish(marker)
def _vision_cb_3D(self, req): res = VisionRequestResponse() if self.last_found_time_3D is None or self.image_sub.last_image_time is None: res.found = False return res dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec() if dt < 0 or dt > self.timeout_seconds: res.found = False elif (self.last3d is None or not self.enabled): res.found = False else: res.pose.header.frame_id = "/map" res.pose.header.stamp = self.last_found_time_3D res.pose.pose.position = numpy_to_point(self.last3d[0]) res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1]) res.found = True return res
def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/front_stereo'): pose = Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0]) ) marker = visualization_msgs.Marker( ns='sub', id=_id, header=mil_ros_tools.make_header(frame=frame), type=visualization_msgs.Marker.SPHERE, action=visualization_msgs.Marker.ADD, pose=pose, color=ColorRGBA(*color), scale=Vector3(*scaling), lifetime=rospy.Duration(), ) self.rviz_pub.publish(marker)
def thrust_cb(self, thrust): ''' Each thrust callback, update the length of the arrow based on the commanded thrust (in newtons). Also update the color of the arrow based on thrust, from green to yellow, with red being reserved for bounds. ''' for cmd in thrust.thruster_commands: if cmd.name not in self.layout: # Don't draw marker if thruster is not in layout continue layout = self.layout[cmd.name] idx = layout['node_id'] bounds = layout['thrust_bounds'] # Select an arrow length based on commanded thrust, max thrust (from layout), and the MAX_LENGTH constant if cmd.thrust < 0: scale = -cmd.thrust / bounds[0] else: scale = cmd.thrust / bounds[1] if np.isclose(scale, 0.0): # Avoid sending 0 length disk-like markers self.markers.markers[idx].action = Marker.DELETE continue else: self.markers.markers[idx].action = Marker.ADD # Set color of marker based on thrust if (cmd.thrust < 0 and cmd.thrust == bounds[0]) or cmd.thrust == bounds[1]: self.markers.markers[idx].color.r = 1.0 self.markers.markers[idx].color.g = 0.0 else: self.markers.markers[idx].color.r = abs(scale) self.markers.markers[idx].color.g = 1.0 # Select endpoint for arrow based on length and direction vector from layout direction = np.array(layout['direction']) direction = direction / np.linalg.norm(direction) pt2 = np.array(layout['position']) + self.MAX_LENGTH * scale * direction self.markers.markers[idx].points[1] = numpy_to_point(pt2) self.markers.markers[idx].header.stamp = rospy.Time.now() self.pub.publish(self.markers)
def make_cylinder_marker(self, base, length, color, frame='/base_link', up_vector=np.array([0.0, 0.0, 1.0]), **kwargs): '''Handle the frustration that Rviz cylinders are designated by their center, not base''' center = base + (up_vector * (length / 2)) pose = Pose( position=mil_ros_tools.numpy_to_point(center), orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0]) ) marker = visualization_msgs.Marker( ns='sub', header=mil_ros_tools.make_header(frame=frame), type=visualization_msgs.Marker.CYLINDER, action=visualization_msgs.Marker.ADD, pose=pose, color=ColorRGBA(*color), scale=Vector3(0.2, 0.2, length), lifetime=rospy.Duration(), **kwargs ) return marker
#!/usr/bin/env python import rospy import numpy as np import tf import mil_ros_tools from sensor_msgs.msg import PointCloud rospy.init_node("ground_finder") pub = rospy.Publisher("ground_est", PointCloud, queue_size=1) listener = tf.TransformListener() pc = PointCloud() pc.header = mil_ros_tools.make_header(frame="map") pc.points = [] rate = rospy.Rate(1.0) while not rospy.is_shutdown(): try: t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1)) (trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("TF waitForTransform timeout") continue pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans))) pub.publish(pc) rate.sleep()
import numpy as np import tf import mil_ros_tools from sensor_msgs.msg import PointCloud rospy.init_node("ground_finder") pub = rospy.Publisher("ground_est", PointCloud, queue_size=1) listener = tf.TransformListener() pc = PointCloud() pc.header = mil_ros_tools.make_header(frame="map") pc.points = [] rate = rospy.Rate(1.0) while not rospy.is_shutdown(): try: t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1)) (trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0)) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("TF waitForTransform timeout") continue pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans))) pub.publish(pc) rate.sleep()