def __init__(self): rospy.init_node('MissionManager') self.Mission = MissionPlan.missionplan.Mission() #print self.Mission; # Variable for waypoint self.waypoint = PointStamped() # Variable for current position of boat--to be set in position_callback self.currPosBoat_LatLon = GeoPointStamped() self.currPosBoat_ECEF = PointStamped() self.currPosBoat_Map = PointStamped() # Set minimum distance to waypoint self.threshold = 3.0 # Receive up-to-date UTM coordinates of boat rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1) # Receive up-to-date lat/lon coordinates of boat rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1) # To publish waypoint updates to MOOS self.wpt_updates = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10) # TODO: # Subscribe to waypoints/behaviors from AutonomousMissionPlanner. Not sure what topic. pass
def forget_ball(self, own=True, team=True, reset_ball_filter=True): """ Forget that we and the best teammate saw a ball, optionally reset the ball filter :param own: Forget the ball recognized by the own robot, defaults to True :type own: bool, optional :param team: Forget the ball received from the team, defaults to True :type team: bool, optional :param reset_ball_filter: Reset the ball filter, defaults to True :type reset_ball_filter: bool, optional """ if own: # Forget own ball self.ball_seen_time = rclpy.Time(seconds=0, nanoseconds=0) self.ball = PointStamped() if team: # Forget team ball self.ball_seen_time_teammate = rclpy.Time(seconds=0, nanoseconds=0) self.ball_teammate = PointStamped() if reset_ball_filter: # Reset the ball filter result = self.reset_ball_filter() if result.success: self.get_logger().info( f"Received message from ball filter: '{result.message}'", logger_name='bitbots_blackboard') else: self.get_logger().warn( f"Ball filter reset failed with: '{result.message}'", logger_name='bitbots_blackboard')
def balls_cb(self, msg): ball_msgs = msg.poses for ball_msg in ball_msgs: ball_point = PointStamped() ball_point.header.frame_id = msg.header.frame_id ball_point.header.stamp = msg.header.stamp ball_point.point = ball_msg.pose.pose.position try: ball_point = self.tf_buffer.transform(ball_point, "base_footprint") except tf2_ros.LookupException: rospy.logwarn("Could not transform from " + msg.header.frame_id + " to 'base_footprint'") return None ball = QGraphicsEllipseItem(0, 0, self.ball_size, self.ball_size, self.radar) ball.setPen(self.ball_pen) ball.setVisible(False) ball.setOpacity(self.opacity) self.set_scaled_position(ball, -1 * ball_point.point.x, -1 * ball_point.point.y, self.ball_size, self.ball_size) color_tuple = self.confidence2color(ball_msg.confidence) ball.setBrush(QBrush(QColor(*color_tuple))) ball.setVisible(self.ball_active)
def transform(self, point, field, stamp): K = self.camera_info.K x = (point.x - K[2]) / K[0] y = (point.y - K[5]) / K[4] z = 1.0 point_on_image = np.array([x, y, z]) intersection = line_plane_intersection(field[0], field[1], point_on_image) if intersection is None: return None intersection_stamped = PointStamped() intersection_stamped.point = intersection intersection_stamped.header.stamp = stamp intersection_stamped.header.frame_id = self.camera_info.header.frame_id try: intersection_transformed = self.tf_buffer.transform( intersection_stamped, self.publish_frame) except tf2_ros.LookupException as ex: rospy.logwarn_throttle( 5.0, "Could not transform from " + self.publish_frame + " to " + intersection_stamped.header.frame_id) rospy.logwarn_throttle(5.0, ex) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle( 5.0, "Waiting for transforms to become available...") rospy.logwarn_throttle(5.0, ex) return None return intersection_transformed.point
def get_detection_based_goal_position_uv(self): """ returns the position of the goal relative to the robot. if only a single post is detected, the position of the post is returned. else, it is the point between the posts :return: """ left = PointStamped(self.goal_odom.header, self.goal_odom.left_post) right = PointStamped(self.goal_odom.header, self.goal_odom.right_post) left.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) right.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) try: left_bfp = self.tf_buffer.transform( left, self.base_footprint_frame, timeout=Duration(seconds=0.2)).point right_bfp = self.tf_buffer.transform( right, self.base_footprint_frame, timeout=Duration(seconds=0.2)).point except tf2.ExtrapolationException as e: self.get_logger().warn(e) self.get_logger().error( 'Severe transformation problem concerning the goal!') return None return (left_bfp.x + right_bfp.x / 2.0), \ (left_bfp.y + right_bfp.y / 2.0)
def _projectPointIntoFrame(self, point, source_frame_id, target_frame_id): """! @brief Transform a point from one frame of reference to another. @param point The geometry_msgs/Point to transform. @param source_frame_id The fully resolved frame_id the point is in reference to. @param target_frame_id The fully resolved frame_id to which the point should be transformed. @return The geometry_msgs/Point in the new frame of reference. """ # First, create a PointStamped, since that is what is needed to transform. point_source_stamped = PointStamped() point_source_stamped.header.stamp = rospy.Time.now() point_source_stamped.header.frame_id = source_frame_id point_source_stamped.point = point # Do the transform. Give a timeout to let it wait for the latest TF if it would # otherwise need to extrapolate. timeout = rospy.Duration(self._tf_timeout) try: point_target_stamped = self._tf_buffer.transform( object_stamped=point_source_stamped, target_frame=target_frame_id, timeout=timeout) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as error: rospy.logerr(msg='TF transform error: {error}'.format(error=error)) raise return point_target_stamped.point
def goal_callback(self, msg): # type: (GoalRelative) -> None self.goal = msg # todo: transform to base_footprint too! # adding a minor delay to timestamp to ease transformations. self.goal.header.stamp = self.goal.header.stamp + rospy.Duration.from_sec( 0.01) goal_left_buffer = PointStamped(self.goal.header, self.goal.left_post) goal_right_buffer = PointStamped(self.goal.header, self.goal.right_post) self.goal_odom.header = self.goal.header if goal_left_buffer.header.frame_id != 'odom': try: self.goal_odom.left_post = self.tf_buffer.transform( goal_left_buffer, 'odom', timeout=rospy.Duration(0.2)).point self.goal_odom.right_post = self.tf_buffer.transform( goal_right_buffer, 'odom', timeout=rospy.Duration(0.2)).point self.goal_odom.header.frame_id = 'odom' self.goal_seen_time = rospy.Time.now() except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: rospy.logwarn(e) else: self.goal_odom.left_post = goal_left_buffer.point self.goal_odom.right_post = goal_right_buffer.point self.goal_seen_time = rospy.Time.now()
def get_detection_based_goal_position_uv(self): """ returns the position of the goal relative to the robot. if only a single post is detected, the position of the post is returned. else, it is the point between the posts :return: """ left = PointStamped(self.goal_odom.header, self.goal_odom.left_post) right = PointStamped(self.goal_odom.header, self.goal_odom.right_post) try: left_bfp = self.tf_buffer.transform( left, 'base_footprint', timeout=rospy.Duration(0.2)).point right_bfp = self.tf_buffer.transform( right, 'base_footprint', timeout=rospy.Duration(0.2)).point except (tf2.ExtrapolationException) as e: rospy.logwarn(e) try: # retrying with latest time stamp available because the time stamp of the goal_odom.header # seems to be to young and an extrapolation would be required. left.header.stamp = rospy.Time(0) right.header.stamp = rospy.Time(0) left_bfp = self.tf_buffer.transform( left, 'base_footprint', timeout=rospy.Duration(0.2)).point right_bfp = self.tf_buffer.transform( right, 'base_footprint', timeout=rospy.Duration(0.2)).point except (tf2.ExtrapolationException) as e: rospy.logwarn(e) rospy.logerr( 'Severe transformation problem concerning the goal!') return None return (left_bfp.x + right_bfp.x / 2.0), \ (left_bfp.y + right_bfp.y / 2.0)
def endPos(msg, rot): # Copying for simplicity position = msg.pose.position quat = msg.pose.orientation #Find difference between camera and suction end diff = PointStamped() diff.header.frame_id = str( msg.header.frame_id) #camera_color_optical_frame diff.header.stamp = rospy.Time(0) diff.point.x = position.x #z diff.point.y = position.y #x diff.point.z = position.z #y p = listener.transformPoint("panda_suction_end", diff) pickPoint = PointStamped() pickPoint.header.frame_id = "panda_suction_end" pickPoint.header.stamp = rospy.Time(0) pickPoint.point.x = p.point.x pickPoint.point.y = p.point.y pickPoint.point.z = p.point.z - suctionCup p = listener.transformPoint("panda_link0", pickPoint) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = rot[0] pose_target.orientation.y = rot[1] pose_target.orientation.z = rot[2] pose_target.orientation.w = rot[3] pose_target.position.x = p.point.x pose_target.position.y = p.point.y pose_target.position.z = p.point.z if (pose_target.position.x > XMAX or pose_target.position.x < XMIN or pose_target.position.y > YMAX or pose_target.position.y < YMIN): rospy.loginfo("Error position out of bounderies") return homePos() return pose_target
def obstacle_cb(self, msg): for item in self.obstacles: self._scene.removeItem(item) for obstacle_msg in msg.obstacles: obstacle = QGraphicsEllipseItem(0, 0, self.obsacle_size, self.obsacle_size, self.radar) obstacle.setPen(self.obstacle_pen) obstacle.setOpacity(self.opacity) obstacle_point = PointStamped() obstacle_point.header.frame_id = msg.header.frame_id obstacle_point.header.stamp = msg.header.stamp obstacle_point.point = obstacle_msg.pose.pose.pose.position try: obstacle_point = self.tf_buffer.transform( obstacle_point, "base_footprint") except tf2_ros.LookupException: rospy.logwarn("Could not transform from " + msg.header.frame_id + " to 'base_footprint'") return None self.set_scaled_position(obstacle, -1 * obstacle_point.point.x, -1 * obstacle_point.point.y, self.obsacle_size, self.obsacle_size) color_tuple = self.confidence2color(obstacle_msg.pose.confidence) obstacle.setBrush(QBrush(QColor(*color_tuple))) obstacle.setVisible(True) self.obstacles.append(obstacle)
def endPos(msg,rot): # Copying for simplicity position = msg.pose.position quat = msg.pose.orientation #Find difference between camera and suction end diff=PointStamped() diff.header.frame_id = str(msg.header.frame_id) #camera_color_optical_frame diff.header.stamp = rospy.Time(0) diff.point.x=position.x #z diff.point.y=position.y #x diff.point.z=position.z #y p=listener.transformPoint("panda_suction_end",diff) pickPoint=PointStamped() pickPoint.header.frame_id = "panda_suction_end" pickPoint.header.stamp = rospy.Time(0) pickPoint.point.x=p.point.x pickPoint.point.y=p.point.y pickPoint.point.z=p.point.z-suctionCup p = listener.transformPoint("panda_link0",pickPoint) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = rot[0] pose_target.orientation.y = rot[1] pose_target.orientation.z = rot[2] pose_target.orientation.w = rot[3] pose_target.position.x = p.point.x pose_target.position.y = p.point.y pose_target.position.z = p.point.z currentbox = [quat.x, quat.y, quat.z, quat.w] print(currentbox) return pose_target, quat
def __init__(self): rospy.init_node('MissionManager') # This will be used to read mission.txt file later... self.Mission = MissionPlan.missionplan.Mission() self.missionReader = MissionReader.MissionReader() # Variable for waypoint - in map / robot coordinates self.waypoint = PointStamped() # Variable for current position of boat--to be set in position_callback self.currPosBoat_LatLon = GeoPointStamped() self.currPosBoat_ECEF = PointStamped() self.currPosBoat_Map = PointStamped() # TODO: Change depth type when we find out what it should be. self.depth = Float32 self.wptIndex = Int32 # TODO: Threshold possibly set thresholds in parameters? # Set minimum distance to waypoint self.shallowWaterDanger = False self.waypointThreshold = 3.0 self.depthThreshold = 4.0 self.shallowWaterNegativeCount = 0 self.shallowWaterPositiveCount = 0 # If ASV receives this many consecutive negative shallow warnings, # reset positive shallowWater counts self.shallowWaterNegativeCountThreshold = 5 # If ASV receives this many consecutive positive shallow warnings, # deviate path and reset negative shallowWater counts self.shallowWaterPositiveCountThreshold = 5 #self.xIndex = 1 self.moos_wptIndex_Update = False self.shallowWaterUpdate = False self.incrementedLine = False # Timer used to print status self.everyTwoSeconds = rospy.Duration(0.5) # Receive up-to-date UTM coordinates of ASV rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1) # Receive up-to-date lat/lon coordinates of ASV rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1) # Receive up-to-date depth rospy.Subscriber('/depth', Float32, self.depth_callback, queue_size = 1) #Receive up-to-date warnings from Don't Run Aground, Ben! behavior. rospy.Subscriber('/shallow_water_warning', String, self.shallow_water_callback, queue_size = 1) # Receive up-to-date waypoint indices rospy.Subscriber('/moos/wpt_index', Int32, self.waypoint_index_callback, queue_size = 1) # Receive up-to-date waypoint updates rospy.Subscriber('/moos/wpt_updates', String, self.waypoint_updates_callback, queue_size = 1) # To publish waypoint updates to MOOS self.wpt_index_publisher = rospy.Publisher('/moos/wpt_index', Int32, queue_size = 10) self.wpt_updates_publisher = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10) self.behavior_ctrl = rospy.Publisher('/behavior_ctrl', BehaviorControl, queue_size = 10)
def publish_marker(self, e): # construct PoseWithCertaintyArray() message for map frame ball_absolute = PoseWithCertainty() ball_absolute.pose.pose = self.pose ball_absolute.confidence = 1.0 balls_absolute = PoseWithCertaintyArray() balls_absolute.header.stamp = rospy.get_rostime() balls_absolute.header.frame_id = "map" balls_absolute.poses = [ball_absolute] # publish the new ball position if self.publish: self.absolute_publisher.publish(balls_absolute) # check if ball is also visible for the robot and publish on relative topic if this is the case # only works if camera info is provided if self.cam_info: try: ball_point_stamped = PointStamped() ball_point_stamped.header.stamp = rospy.Time.now() ball_point_stamped.header.frame_id = "map" ball_point_stamped.point = ball_absolute.pose.pose.position ball_in_camera_optical_frame = tf_buffer.transform( ball_point_stamped, self.cam_info["frame_id"], timeout=rospy.Duration(0.5)) if ball_in_camera_optical_frame.point.z >= 0: p = [ ball_in_camera_optical_frame.point.x, ball_in_camera_optical_frame.point.y, ball_in_camera_optical_frame.point.z ] k = np.reshape(self.cam_info["K"], (3, 3)) p_pixel = np.matmul(k, p) p_pixel = p_pixel * (1 / p_pixel[2]) # make sure that the transformed pixel is inside the resolution and positive. if 0 < p_pixel[0] <= self.cam_info["width"] and 0 < p_pixel[ 1] <= self.cam_info["height"]: ball_in_footprint_frame = tf_buffer.transform( ball_in_camera_optical_frame, "base_footprint", timeout=rospy.Duration(0.5)) ball_relative = PoseWithCertainty() ball_relative.pose.pose.position = ball_in_footprint_frame.point ball_relative.confidence = 1.0 balls_relative = PoseWithCertaintyArray() balls_relative.header = ball_in_footprint_frame.header balls_relative.poses = [ball_relative] self.relative_publisher.publish(balls_relative) except tf2_ros.LookupException as ex: rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex)) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex)) return None
def transform(self, value): point_new = geometry_msgs.msg.PointStamped() point_new.header = value.req.header point_new.point = value.req.point transformed = self.tfBuffer.transform(point_new, self.camera_model.tfFrame()) transformed_new = PointStamped() transformed_new.header = transformed.header transformed_new.point = transformed.point return transformed_new
def detect_fun(self): found = False sherd_poses = [] # initialize empty # confirm that color mask exists if self.color_mask is None: rospy.logwarn('AutoCore: No color mask received.') return found, sherd_poses # run segment_sherds.py on what robot sees in this position req = SherdDetectionsRequest() req.color_mask = self.color_mask try: res = self.detection_srv(req) except rospy.ServiceException as e: rospy.logerr( 'AutoCore: Bounding box service call failed: {}'.format(e)) raise else: detections = res.detections.detections rospy.logdebug('Bounding boxes message: {}'.format(detections)) if detections: found = True # TODO return (first or random or smallest etc.) sherd from list for item in detections: point_cam = PointStamped() # build ROS message for conversion point_cam.header = item.header point_cam.point.x, point_cam.point.y, point_cam.point.z = item.bbox.center.position.x, item.bbox.center.position.y, item.bbox.center.position.z try: point_base = self.tfBuffer.transform( point_cam, 'base_link') except tf2_ros.buffer_interface.TypeException as e: rospy.logerr('AutoCore: tf failure: {}'.format(e)) raise rospy.logdebug( 'Obtained transform between camera_link and base_link.') rospy.logdebug( 'Sherd center point (x,y,z) [m] in base_link frame: {}'. format(point_base)) sherd_poses.append({ 'position': np.array([ point_base.point.x, point_base.point.y, point_base.point.z ]), 'roll': item.bbox.center.roll }) rospy.logdebug('sherds list = {}'.format(sherd_poses)) return found, sherd_poses
def write_circles(self, data): object_frame = rospy.get_param("~object_frame") global_frame = rospy.get_param( "~global_frame") #NE MOZE MAPA BITI OBJECT... tf = self.tfBuffer.lookup_transform(global_frame, object_frame, rospy.Time(0)) orientation = tf.transform.rotation quat_list = [ orientation.x, orientation.y, orientation.z, orientation.w ] (roll, pitch, yaw) = euler_from_quaternion(quat_list) # print yaw newData = Obstacles() #for publishing transformed circles newData.circles = [] newData.header = data.header newData.header.frame_id = global_frame self.circles = [] temp_point_stamped = PointStamped() for circle in data.circles: # x = self.pioneerPose.position.x + math.cos(yaw) * circle.center.x + math.sin(yaw) * circle.center.y # y = self.pioneerPose.position.y - math.sin(yaw) * circle.center.x + math.cos(yaw) * circle.center.y # circle.center.x = x # circle.center.y = y temp_point_stamped.point = circle.center new_center = tf2_geometry_msgs.do_transform_point( temp_point_stamped, tf) circle.center = new_center.point self.circles.append(circle) newData.circles.append(circle) self.transformed_obstacles.publish(newData) self.circles = data.circles max_num_of_no_actor = rospy.get_param("~max_num_of_no_actor") self.header = data.header self.header.frame_id = global_frame if self.actor != None: self.update_actor() if self.count >= max_num_of_no_actor: self.stop_robot() rospy.signal_shutdown("Actor has been lost!!!")
def get_plane(self, stamp, object_height, base_frame): """ returns a plane which an object is believed to be on as a tuple of a point on this plane and a normal""" field_normal = PointStamped() field_normal.header.frame_id = base_frame field_normal.header.stamp = stamp field_normal.point.x = 0.0 field_normal.point.y = 0.0 field_normal.point.z = 1.0 try: field_normal = self.tf_buffer.transform( field_normal, self.camera_info.header.frame_id, timeout=rospy.Duration(0.2)) except tf2_ros.LookupException as ex: rospy.logwarn_throttle( 5.0, "Could not transform from " + base_frame + " to " + self.camera_info.header.frame_id) rospy.logwarn_throttle(5.0, ex) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle( 5.0, "Waiting for transforms to become available...") rospy.logwarn_throttle(5.0, ex) return None field_point = PointStamped() field_point.header.frame_id = base_frame field_point.header.stamp = stamp field_point.point.x = 0.0 field_point.point.y = 0.0 field_point.point.z = object_height try: field_point = self.tf_buffer.transform( field_point, self.camera_info.header.frame_id) except tf2_ros.LookupException as ex: rospy.logwarn_throttle( 5.0, "Could not transform from " + base_frame + " to " + self.camera_info.header.frame_id) rospy.logwarn_throttle(5.0, ex) return None field_normal = np.array( [field_normal.point.x, field_normal.point.y, field_normal.point.z]) field_point = np.array( [field_point.point.x, field_point.point.y, field_point.point.z]) # field normal is a vector! so it stats at field point and goes up in z direction field_normal = field_point - field_normal return field_normal, field_point
def ball_filtered_callback(self, msg: PoseWithCovarianceStamped): self.ball_filtered = msg # When the precision is not sufficient, the ball ages. x_sdev = msg.pose.covariance[0] # position 0,0 in a 6x6-matrix y_sdev = msg.pose.covariance[7] # position 1,1 in a 6x6-matrix if x_sdev > self.body_config['ball_position_precision_threshold']['x_sdev'] or \ y_sdev > self.body_config['ball_position_precision_threshold']['y_sdev']: self.forget_ball(own=True, team=False, reset_ball_filter=False) return ball_buffer = PointStamped(msg.header, msg.pose.pose.position) try: self.ball = self.tf_buffer.transform(ball_buffer, self.base_footprint_frame, timeout=Duration(seconds=0.3)) self.ball_odom = self.tf_buffer.transform( ball_buffer, self.odom_frame, timeout=Duration(seconds=0.3)) self.ball_map = self.tf_buffer.transform( ball_buffer, self.map_frame, timeout=Duration(seconds=0.3)) # Set timestamps to zero to get the newest transform when this is transformed later self.ball_odom.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) self.ball_map.header.stamp = rclpy.Time(seconds=0, nanoseconds=0) self.ball_seen_time = self.get_clock().now() self.ball_publisher.publish(self.ball) self.ball_seen = True except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self.get_logger().warn(e)
def goToGoal(self, goal_position): stamped_goal = PointStamped() stamped_goal.header.frame_id = "odom" stamped_goal.point.x = goal_position.x stamped_goal.point.y = goal_position.y # Attend exercise requirements tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) rate = rospy.Rate(10.0) while not self.reachTheGoal(): try: trans = tf_buffer.transform(stamped_goal, "base_link") except: rate.sleep() print "Error getting transform" continue vel_msg = Twist() print "vel_msg.linear.x = " + str( self.linearVelocity(trans)) + " vel_msg.angular.z = " + str( self.angularVelocity(trans)) vel_msg.linear.x = self.linearVelocity(trans) vel_msg.angular.z = self.angularVelocity(trans) self.velocity_publisher.publish(vel_msg) rate.sleep()
def __init__(self, field_length, field_width, goal_width): self.position = Position2D() self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(30)) self.tf_listener = tf2.TransformListener(self.tf_buffer) self.ball = PointStamped() # The ball in the base footprint frame self.goal = GoalRelative() self.goal_odom = GoalRelative() self.goal_odom.header.stamp = rospy.Time.now() self.goal_odom.header.frame_id = 'odom' self.obstacles = ObstaclesRelative() self.my_data = dict() self.counter = 0 self.ball_seen_time = rospy.Time(0) self.goal_seen_time = rospy.Time(0) self.ball_seen = False self.field_length = field_length self.field_width = field_width self.goal_width = goal_width # Publisher for Visualisation in RViZ self.ball_publisher = rospy.Publisher('/debug/viz_ball', PointStamped, queue_size=1) self.goal_publisher = rospy.Publisher('/debug/viz_goal', GoalRelative, queue_size=1)
def handle_gaze_controller(self, event): with self.lock: try: aaa = PointStamped() aaa.header.stamp = rospy.Time() aaa.header.frame_id = self.target.target_point.header.frame_id aaa.point.x = self.target.target_point.point.x aaa.point.y = self.target.target_point.point.y aaa.point.z = self.target.target_point.point.z point_transformed = self.tf_buf.transform(aaa, 'head_base') except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): e = sys.exc_info()[0] rospy.logdebug(e) rospy.logwarn( "[%s] Can't tranform from gaze to target.[ %s - %s ]" % (rospy.get_name(), 'gaze', self.target.target_point.header.frame_id)) return pan_angle = math.atan2(point_transformed.point.y, point_transformed.point.x) tilt_angle = math.atan2(point_transformed.point.z, point_transformed.point.x) # print pan_angle * 180 / math.pi, tilt_angle * 180 / math.pi if self.enable_gaze: self.pub_pan.publish(pan_angle) self.pub_tilt.publish(tilt_angle)
def __init__(self): self.lock = threading.RLock() self.tf_buf = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buf) self.prev_head_pan = 0.0 self.enable_gaze_sync = True self.result_pan_angle = 0.0 self.result_tilt_angle = 0.0 self.sub_gaze_sync = rospy.Subscriber('set_gaze_sync', Bool, self.handle_set_gaze_sync) self.sub_gaze_target = rospy.Subscriber('set_gaze_target', PointStamped, self.handle_set_gaze_target) self.pub_head_pan = rospy.Publisher('/head/pan_controller/command', Float64, queue_size=10) self.pub_head_tilt = rospy.Publisher('/head/tilt_controller/command', Float64, queue_size=10) with self.lock: self.head_target_point = PointStamped() self.head_target_point.header.frame_id = "base_footprint" self.head_target_point.point.x = 2.0 self.head_target_point.point.y = 0.0 self.head_target_point.point.z = 1.291 rospy.Timer(rospy.Duration(0.02), self.handle_head_controller) rospy.loginfo('[%s] initialzed...' % rospy.get_name())
def _transform_point(self, point: Point, field, stamp) -> Point: np_point = self._get_field_intersection_for_pixels( np.array([[point.x, point.y, point.z]]), field)[0] if np.isnan(np_point).any(): return None intersection_stamped = PointStamped() intersection_stamped.point.x = np_point[0] intersection_stamped.point.y = np_point[1] intersection_stamped.point.z = np_point[2] intersection_stamped.header.stamp = stamp intersection_stamped.header.frame_id = self._camera_info.header.frame_id try: intersection_transformed = self._tf_buffer.transform( intersection_stamped, self._publish_frame) except tf2_ros.LookupException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None return intersection_transformed.point
def gripper_in_odom(self): success = False # Point of gripper origin. gripper_position = PointStamped() # TODO Should this be 'time'? gripper_position.header.stamp = rospy.Time.now() gripper_position.header.frame_id = "wrist_3_link" gripper_position.point.x = 0 gripper_position.point.y = 0 gripper_position.point.z = 0 print("Created gripper origin for transformation.") try: # TODO Need to deal with if time is 0 because the clock hasn't been published yet(?)? - http://wiki.ros.org/roscpp/Overview/Time # Times out after 1 second. What happens if the buffer doesn't contain the # transformation after this duration? [I think it throws an error] # --> Even works with a duration of 0.001 - how does this work? TODO gripper_odom = self.tf_buffer.transform(gripper_position, 'odom', rospy.Duration(1)) print("Performed transform") success = True except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: # TODO Need to add more details to this. Might just be doing it because the buffer isn't large enough yet. # TODO Might need to revise this error message now that I've chopped things around. print("Strategy TF transform error. Message:: {}".format(e)) # TODO Return success variable? return [gripper_odom.point.x, gripper_odom.point.y]
def get_plane(self, stamp, object_height): """ returns a plane which an object is believed to be on as a tuple of a point on this plane and a normal""" base_frame = self._base_footprint_frame field_normal = PointStamped() field_normal.header.frame_id = base_frame field_normal.header.stamp = stamp field_normal.point.x = 0.0 field_normal.point.y = 0.0 field_normal.point.z = 1.0 try: field_normal = self._tf_buffer.transform( field_normal, self._camera_info.header.frame_id, timeout=rospy.Duration(0.2)) except tf2_ros.LookupException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None field_point = PointStamped() field_point.header.frame_id = base_frame field_point.header.stamp = stamp field_point.point.x = 0.0 field_point.point.y = 0.0 field_point.point.z = object_height try: field_point = self._tf_buffer.transform( field_point, self._camera_info.header.frame_id) except tf2_ros.LookupException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return None field_normal = np.array( [field_normal.point.x, field_normal.point.y, field_normal.point.z]) field_point = np.array( [field_point.point.x, field_point.point.y, field_point.point.z]) # field normal is a vector! so it stats at field point and goes up in z direction field_normal = field_point - field_normal return field_normal, field_point
def draw_goal_part(self, obj, msg, point, size): goal_point = PointStamped() goal_point.header.frame_id = msg.header.frame_id goal_point.header.stamp = msg.header.stamp goal_point.point = point try: goal_point = self.tf_buffer.transform(goal_point, "base_footprint") except tf2_ros.LookupException: rospy.logwarn("Could not transform from " + msg.header.frame_id + " to 'base_footprint'") return None self.set_scaled_position(obj, -1 * goal_point.point.x, -1 * goal_point.point.y, size, size) color_tuple = self.confidence2color(msg.confidence) obj.setBrush(QBrush(QColor(*color_tuple))) obj.setVisible(self.goal_active)
def perform(self, connector, reevaluate=False): point = PointStamped() point.point.x = self.u point.point.y = self.v point.point.z = self.w point.header.frame_id = 'base_footprint' self.look_at(point, connector)
def msg_to_pos(tf_buffer, msg, to_frame_id, scale=1.0): if msg is None: return None ps = PointStamped() ps.header = msg.header ps.point.x = msg.pose.pose.position.x * scale ps.point.y = msg.pose.pose.position.y * scale ps.point.z = msg.pose.pose.position.z * scale try: ps2 = tf_buffer.transform(ps, to_frame_id) except Exception as e: rospy.logwarn('Exception during TF from {:s} to {:s}: {:s}'.format( msg.header.frame_id, to_frame_id, e)) return None return np.array([ps2.point.x, ps2.point.y, ps2.point.z])
def ball_cb(self, msg): ball_point = PointStamped() ball_point.header.frame_id = msg.header.frame_id ball_point.header.stamp = msg.header.stamp ball_point.point = msg.ball_relative try: ball_point = self.tf_buffer.transform(ball_point, "base_footprint") except tf2_ros.LookupException: rospy.logwarn("Could not transform from " + msg.header.frame_id + " to 'base_footprint'") return None if msg.ball_relative is not None: self.set_scaled_position(self.ball, -1 * ball_point.point.x, -1 * ball_point.point.y, self.ball_size, self.ball_size) color_tuple = self.confidence2color(msg.confidence) self.ball.setBrush(QBrush(QColor(*color_tuple))) self.ball.setVisible(self.ball_active)
def balls_relative_cb(self, msg): if msg.poses: balls = sorted(msg.poses, reverse=True, key=lambda ball: ball.confidence) # Sort all balls by confidence ball = balls[0] # Ball with highest confidence # Get ball relative to base footprint ball_obj = PointStamped() ball_obj.header = msg.header ball_obj.point = ball x, y = self.get_ball_position_uv(ball_obj) self.ball_position = (float(x), float(y)) # Set ball distance self.ball_distance = math.sqrt(float(self.ball_position[0])**2 + float(self.ball_position[1])**2) # Calculate the angle in which we are facing the ball self.ball_angle = math.atan2(float(self.ball_position[1]),float(self.ball_position[0])) # Set the refresh time self.ball_age = time.time() else: rospy.logwarn("Received empty balls message")