def poseCallback(pose): try: trans = tfBuffer.lookup_transform(pose.header.frame_id, "map", rospy.Time(0)) newPose = tf2_geometry_msgs.do_transform_pose(pose,trans) pub.publish(newPose) rospy.loginfo(newPose) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logerr("exception thrown %s", e) return
def transform_pose(pose, tf): """ Transform the given pose with the given transform """ # do_transform_pose expects a stamped pose, but it ignores the header if isinstance(pose, geometry_msgs.Pose2D): p = to_pose3d( pose, frame='dummy') # just force to_pose3d return a stamped pose elif isinstance(pose, geometry_msgs.Pose): p = geometry_msgs.PoseStamped(None, pose) elif isinstance(pose, geometry_msgs.PoseStamped): p = pose else: raise rospy.ROSException( "Input parameter pose is not a valid geometry_msgs pose object") return tf2_geometry_msgs.do_transform_pose(p, tf)
def receivePose(self, pose): transform = self.tf_buffer.lookup_transform("world2D", pose.header.frame_id, #source frame rospy.Time(0), #get the tf at first available time rospy.Duration(1.0)) #wait for 1 second self.robotPTAMPose = tf2_geometry_msgs.do_transform_pose(pose, transform).pose #self.robotPTAMPose = pose.pose self.rlSafePath.header.stamp = rospy.Time.now() self.safe_path_pub.publish(self.rlSafePath) self.rlUnsafePath.header.stamp = rospy.Time.now() self.unsafe_path_pub.publish(self.rlUnsafePath) self.lookahead_path_pub.publish(self.lookaheadPath)
def transform_coordinates(self, coord, from_frame='base_scan', to_frame='odom'): ''' Gets frame transform at latest time ''' p1 = PoseStamped() p1.header.frame_id = from_frame p1.pose.position.x = coord[0] p1.pose.position.y = coord[1] # Not sure this is right p1.pose.position.z = coord[2] p1.pose.orientation.w = 1.0 # Neutral orientation transform = self.tf_buffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0)) return tf2_geometry_msgs.do_transform_pose(p1, transform)
def base_scan_pose(self): base_scan_position = {"translation": {}, "rotation": {}} pose_transformed = tf2_geometry_msgs.do_transform_pose( self.pose, self.transform_data) print(pose_transformed) for i in ['x', 'y', 'z']: # exec(self.pose.pose.position."+ i +"+ self.transform_data.translation."+ i) # exec("self.pose.pose.orientation."+ i +"+ self.transform_data.rotation."+ i) base_scan_position["translation"][i] = getattr( pose_transformed.pose.position, i) for u in ['x', 'y', 'z', 'w']: base_scan_position["rotation"][u] = getattr( pose_transformed.pose.orientation, u) return base_scan_position
def displacement_from_target(self, target=None, current=None, frame_id=None): if target is None: target = self._setpoint_pos if current is None: current = self.get_position() if frame_id is not None: transform1 = self.tf_buffer.lookup_transform( frame_id, target.header.frame_id, rospy.Time(0), rospy.Duration(1)) transform2 = self.tf_buffer.lookup_transform( frame_id, current.header.frame_id, rospy.Time(0), rospy.Duration(1)) target = tf2_geometry_msgs.do_transform_pose(target, transform1) current = tf2_geometry_msgs.do_transform_pose(current, transform2) # Should probably allow Pose or PoseStamped objects target = target.pose.position current = current.pose.position return Vector3(target.x - current.x, target.y - current.y, target.z - current.z)
def pose_callback(self, msg): rospy.loginfo("Received at goal message!") # Copying for simplicity position = msg.pose.position quat = msg.pose.orientation rospy.loginfo("Point Position: [ %f, %f, %f ]" % (position.x, position.y, position.z)) rospy.loginfo("Quat Orientation: [ %f, %f, %f, %f]" % (quat.x, quat.y, quat.z, quat.w)) transform = self.tf_buffer.lookup_transform( 'base', msg.header.frame_id, #source frame rospy.Time(0), #get the tf at first available time rospy.Duration(1.0)) #wait for 1 second pose_transformed = tf2_geometry_msgs.do_transform_pose(msg, transform) self.marker.header.frame_id = pose_transformed.header.frame_id self.marker.type = self.marker.ARROW self.marker.action = self.marker.ADD self.marker.scale.x = 0.25 self.marker.scale.y = 0.03 self.marker.scale.z = 0.03 self.marker.color.a = 1.0 self.marker.color.r = 1.0 self.marker.color.g = 1.0 self.marker.color.b = 0.0 self.marker.pose.orientation = pose_transformed.pose.orientation self.marker.pose.position = pose_transformed.pose.position self.marker.id = 0 hdr = msg.header poses = { 'right': pose_transformed, } # self.flagPub.publish(str(self._performedMotion)) if not self._performedMotion: self._performedMotion = True succ, resp = self.ik_solver(poses) if succ: rospy.loginfo("Simple IK call passed!") print "Moving to joint location" joint_pos = np.array(resp.joints[0].position) self.move_to_joint_position(joint_pos) else: rospy.logerr("Simple IK call FAILED")
def check_direction(cj_injection_message): global listener, tfBuffer trans = None try: trans = tfBuffer.lookup_transform('world', prefix, rospy.Time(0)) except: rospy.logwarn("tf lookup -- {} not found".format(prefix)) if trans != None: cj_local_coord = PoseStamped() cj_local_coord = tf2_geometry_msgs.do_transform_pose( cj_injection_message, trans) rospy.loginfo(cj_local_coord) heading = atan2(cj_local_coord.pose.position.y, cj_local_coord.pose.position.x) rospy.loginfo(heading)
def transform_3D_point_to_world_frame(self, xyz_point_from_sensor, sensor_frame_id): pose_stamped = geometry_msgs.msg.PoseStamped() pose_stamped.header.frame_id = sensor_frame_id pose_stamped.pose.orientation.w = 1.0 pose_stamped.pose.position.x = xyz_point_from_sensor[0] pose_stamped.pose.position.y = xyz_point_from_sensor[1] pose_stamped.pose.position.z = xyz_point_from_sensor[2] transform = self.tf_buffer.lookup_transform( "world", pose_stamped.header.frame_id, rospy.Time(0), rospy.Duration(1.0)) pose_transformed = tf2_geometry_msgs.do_transform_pose( pose_stamped, transform) return pose_transformed
def main(): try: tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) #tf buffer length tf_listener = tf2_ros.TransformListener(tf_buffer) tf_transform = tf_buffer.lookup_transform( "world", "dumb_link_2", #source frame rospy.Time(0), #get the tf at first available time rospy.Duration(4.0)) pose_transformed = tf2_geometry_msgs.do_transform_pose( bottle_pose, tf_transform) print(pose_transformed) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("tf not working")
def transform_pose(pose, trans, rot): t = TransformStamped() t.transform.translation.x = trans[0] t.transform.translation.y = trans[1] t.transform.translation.z = trans[2] t.transform.rotation.x = rot[0] t.transform.rotation.y = rot[1] t.transform.rotation.z = rot[2] t.transform.rotation.w = rot[3] pose_stamped = PoseStamped() pose_stamped.pose = pose pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, t) return pose_transformed.pose
def map_to_odom_transform(): """ This method constantly updates the offset of the map and odometry coordinate systems based on the latest results from the localizer """ global new_translation, new_rotation (translation, rotation) = convert_pose_inverse_transform(robot_pose) map_wrt_base_pose = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation)) map_wrt_base_pose.header.stamp=rospy.Time.now() map_wrt_base_pose.header.frame_id=base_frame # access/look-up the transform odom (target/new frame) wrt base(source/current frame): # tf_buffer.lookup_transform(target_frame, source_frame, query_time, timeout_secs) ts_odom_wrt_base = tf_buffer.lookup_transform(odom_frame, base_frame, rospy.Time(), rospy.Duration(1.0)) # wait 1s for transform to become available map_wrt_odom_pose = tf2_geometry_msgs.do_transform_pose(map_wrt_base_pose, ts_odom_wrt_base) # invert map_wrt_odom_to get odom_wrt_map and store as tuple position, quaternion (new_translation, new_rotation) = convert_pose_inverse_transform(map_wrt_odom_pose)
def transform_object_pose_to_robot_rf(self): #kinect camera axis not the same as the robot axis so we could have #to perform the necessary transforms first to get both axes aligned #and then to transform camera rf to robot's rf #goal_pose is the final pose of the marker wrt the robot's rf tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) tf_listener = tf2_ros.TransformListener(tf_buffer) transform = tf_buffer.lookup_transform('base', 'camera_link', rospy.Time(0), rospy.Duration(1.0)) trans_pose = tf2_geometry_msgs.do_transform_pose( self.object_pose, transform) return trans_pose
def send_goal(goal_pt): global goal, wp_tolerance try: transform = tf_buffer.lookup_transform('odom', 'map', rospy.Time(0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return pose_stamped = PoseStamped() pose_stamped.header.frame_id = 'map' pose_stamped.pose.position.x = goal_pt[0] pose_stamped.pose.position.y = goal_pt[1] pose_transformed = tf2_geometry_msgs.do_transform_pose( pose_stamped, transform) odom_point = PointStamped() # odom_point.header.frame_id = "odom" # odom_point.point.x = pose_transformed.pose.position.x # odom_point.point.y = pose_transformed.pose.position.y odom_point.header.frame_id = "map" odom_point.point.x = goal_pt[0] odom_point.point.y = goal_pt[1] odom_point.point.z = 0 odom_point.header.stamp = rospy.Time.now() gps_pub.publish(odom_point) # print odom_point oldX = goal.target_pose.pose.position.x oldY = goal.target_pose.pose.position.y newX = pose_transformed.pose.position.x newY = pose_transformed.pose.position.y dist_between_goals = math.sqrt( math.pow(oldX - newX, 2) + math.pow(oldY - newY, 2)) # print dist_between_goals,wp_tolerance if dist_between_goals > wp_tolerance: goal.target_pose.header.frame_id = "odom" goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = newX goal.target_pose.pose.position.y = newY goal.target_pose.pose.orientation.x = 0 goal.target_pose.pose.orientation.y = 0 goal.target_pose.pose.orientation.z = 0 goal.target_pose.pose.orientation.w = 1 client.send_goal(goal) print "Goal sent" print goal
def callback_velocity(self, msg): # rospy.loginfo("Received dimmensions and velocities msg") # Logger the received msg flag = False pose_input = PoseStamped() pose_input.header = 'base_link' pose_input.pose = msg.pose # Transform the pose in car frame to the map frame so we can show the right direction try: transform = self.tf_buffer.lookup_transform( 'map', 'base_link', #source frame rospy.Time(0), #get the tf at first available time rospy.Duration(1.0)) #wait for 1 second flag = True pose_transformed = tf2_geometry_msgs.do_transform_pose( pose_input, transform) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): flag = False pose_transformed = msg.pose # Calculate the velocity from axis moving_vel = math.sqrt(msg.vel_x**2 + msg.vel_y**2) # Check if the object is a moving object if moving_vel > STATIC_VEL_OBJ: x = pose_transformed.pose.position.x y = pose_transformed.pose.position.y if x > 0: x_dir = 'north' else: x_dir = 'south' if y > 0: y_dir = 'west' else: y_dir = 'east' # Check if the object is bigger than 2 m each side then must be a moving car if msg.dimension.x > 2 or msg.dimension.y > 2: obj_type = 'car' else: obj_type = 'person' # Log the moving objects rospy.loginfo( 'There is a %s moving in direction %s %s with velocity %3f m/s' % (obj_type, x_dir, y_dir, moving_vel))
def MoveHand(x, y, z, xO, yO, zO, side, time): if m.isnan(x): print Cont.O.BOLD + Cont.O.UNDERLINE + Cont.O.RED + "NAN Value received, Not Moving Arms" + Cont.O.END return False try: global transform, transformP global tf_buffer transform = tf_buffer.lookup_transform("world", "head", rospy.Time(0), rospy.Duration(1.0)) headPoint = PoseStamped() headPoint.header.frame_id = "head" headPoint.pose.position.x = x headPoint.pose.position.y = y headPoint.pose.position.z = z worldPoint = tf2_geometry_msgs.do_transform_pose(headPoint, transform) POSITION = [ worldPoint.pose.position.x, worldPoint.pose.position.y, worldPoint.pose.position.z ] transformP = tf_buffer.lookup_transform("world", "pelvis", rospy.Time(0), rospy.Duration(1.0)) temp = tf.transformations.euler_from_quaternion( (transformP.transform.rotation.x, transformP.transform.rotation.y, transformP.transform.rotation.z, transformP.transform.rotation.w)) ORIENTATION = [xO + temp[0], yO + temp[1], zO + temp[2]] quat = tf.transformations.quaternion_from_euler( ORIENTATION[0], ORIENTATION[1], ORIENTATION[2]) Cont.aC.HandMsgMaker(time, side, POSITION, quat) print Cont.O.color.GREEN + "Published Hand Trajectory" + Cont.O.color.END stuff = String() stuff.data = "Published Hand Trajectory" conn.publish(stuff) POINT = PointStamped() POINT.point.x = POSITION[0] POINT.point.y = POSITION[1] POINT.point.z = POSITION[2] POINT.header.frame_id = "world" PointPUB.publish(POINT) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print "HELP" rate.sleep()
def transform_microwave_marker_pose_to_robot_rf(self): #kinect camera axi is not the same as the robot axis so we could have #to perform the necessary transforms first to get both axes aligned #and then to transform camera rf to robot's rf #goal_pose is the final pose of the marker wrt the robot's rf marker_pose = PoseStamped() marker_pose.pose.position.y = self.microwave_marker_pose.pose.position.y marker_pose.pose.position.x = self.microwave_marker_pose.pose.position.x marker_pose.pose.position.z = self.microwave_marker_pose.pose.position.z marker_pose.pose.orientation = self.microwave_marker_pose.pose.orientation tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) tf_listener = tf2_ros.TransformListener(tf_buffer) transform = tf_buffer.lookup_transform('base', 'camera_link',rospy.Time(0), rospy.Duration(1.0)) self.microwave_goal_pose = tf2_geometry_msgs.do_transform_pose(marker_pose, transform)
def separator_on_shelf_layer(self, separator_pose, width_threshold=0.035, height_threshold=0.06): """ :param separator_pose: pose of separator in floor frame :type separator_pose: PoseStamped :param width_threshold: all separators that are this close to the width edge are filtered. :type width_threshold: float :type height_threshold: float :return: bool """ separator_pose = do_transform_pose(separator_pose, self.T_map___layer) x = separator_pose.pose.position.x z = separator_pose.pose.position.z return width_threshold <= x and x <= self.current_shelf_layer_width - width_threshold and \ -height_threshold <= z and z <= height_threshold
def arm_homing(): global arm jointTarget = [0.947, 5.015, 4.95, 1.144, 11.425, 4.870, 7.281] arm.move_to_joint_pose(jointTarget) rospy.sleep(3) transform = tfBuffer.lookup_transform('linear_actuator_link', 'right_ee_link', rospy.Time(0), rospy.Duration(1.0)) ps = geometry_msgs.msg.PoseStamped() ps.header.stamp = rospy.Time.now() pt = tf2_geometry_msgs.do_transform_pose(ps, transform) pt.pose.orientation.w = 1.0 pt.pose.orientation.x = 0.0 pt.pose.orientation.y = 0.0 pt.pose.orientation.z = 0.0 pt.pose.position.z += 0.1 return pt.pose
def transformPose(self, out_frame_id, pose_in): # in tf2, frames do not have the initial slash if (pose_in.header.frame_id[0] == '/'): pose_in.header.frame_id = pose_in.header.frame_id[1:] # in tf2, frames do not have the initial slash if (out_frame_id[0] == '/'): out_frame_id = out_frame_id[1:] try: transform = self.listenerBuffer.lookup_transform(out_frame_id, pose_in.header.frame_id, rospy.Time.now(), rospy.Duration(4.0)) pose_out = do_transform_pose(pose_in, transform) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: rospy.logwarn("[%s] transform from (%s) to (%s) failed: (%s).", rospy.get_name(), pose_in.header.frame_id, out_frame_id, e) pose_out = None return pose_out
def transform(self, passed_stamped_pose): """ Apply a transform to a given pose. """ # Creating / Updating transform with latest translation and rotation. transform = TransformStamped() transform.header = rospy.get_rostime() transform.transform.translation = Point(self.translation[0], self.translation[1], 0.0) transform.transform.rotation = Quaternion(self.rotation[0], self.rotation[1], self.rotation[2], self.rotation[3]) # pose = PoseStamped(passed_stamped_pose.header, passed_stamped_pose.pose) pose = tf2_geometry_msgs.do_transform_pose(passed_stamped_pose, transform) return pose
def grasp(self, msg): camera_frame_pose = geometry_msgs.msg.PoseStamped() camera_frame_pose.header = msg.header camera_frame_pose.pose = msg.poses[0] print camera_frame_pose transform = self.tf_buffer.lookup_transform('base', 'head_camera_fixed', msg.header.stamp, rospy.Duration(1.0)) pose_transformed = tf2_geometry_msgs.do_transform_pose(camera_frame_pose, transform) print pose_transformed roll, pitch, yaw = tf.transformations.euler_from_quaternion([0, 0, 1, 0]) roll += 0 pitch += -1.57 yaw += 1.57 print roll, pitch, yaw quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) print quat # pose = [0.5, 0.2, 0.2] pose = [pose_transformed.pose.position.x, pose_transformed.pose.position.y, 0.4] if not self._move_arm_to_pose(pose, quat): rospy.logwarn("Planning failed. Exiting") return pose[2] -= 0.2 self._move_arm_to_pose(pose, quat) self.gripper.command_position(0, block=True) pose[2] += 0.2 self._move_arm_to_pose(pose, quat) rospy.sleep(2) pose[2] -= 0.2 self._move_arm_to_pose(pose, quat) self.gripper.command_position(100, block=True) pose[2] += 0.2 self._move_arm_to_pose(pose, quat) self._move_arm_to_pose(self.home_pose, quat) print "Finished" self.subscriber.unregister()
def transform_trajectory(trajectory, transformations): ''' translate each point in trajectory by a transformation interpolated to the correct time, return the transformed trajectory''' # for each point in trajectory, find the interpolated transformation, then transform the trajectory point matching_transforms = get_interpolations(trajectory, transformations) res = [] for i in range(len(matching_transforms)): trans = matching_transforms[i] traj_pose = trajectory[i] transformed = tf2_geometry_msgs.do_transform_pose(traj_pose, trans) res.append(transformed) return res
def targetUpdate( data ): #transforms target to map frame as that is the only frame guaranteed to be static global target transf = tfBuffer.lookup_transform(MAP_FRAME, data.header.frame_id, data.header.stamp, timeout=rospy.Duration(2)) data = tf2_geometry_msgs.do_transform_pose(data, transf) target = data target.header.frame_id = MAP_FRAME global controller_done controller_done = False global near_target near_target = False print("received new target: (map_frame)" + str(target)) pass
def getRobotPoseSt(): global tfBuffer, listener destFrame = "world" inPoseStamped = PoseStamped() inPoseStamped.pose.orientation.w = 1 inPoseStamped.header.stamp = rospy.Time.now() inPoseStamped.header.frame_id = "actor00/base_link" absPoseStamped = None t0 = rospy.Time() try: trans = tfBuffer.lookup_transform(destFrame, inPoseStamped.header.frame_id, t0, rospy.Duration(5.0)) absPoseStamped = tf2_geometry_msgs.do_transform_pose(inPoseStamped, trans) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: rospy.logerr("TF between %s and %s not ready: [%s]" % (destFrame, inPoseStamped.header.frame_id, str(e))) return absPoseStamped
def on_enter(self, userdata): self.pose_transformed = None tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) trans = tf_buffer.lookup_transform(self._after, self._before, rospy.Time(0), rospy.Duration(1.0)) before_pose = userdata.target_pose pose = PoseStamped() pose.pose = before_pose transformed = tf2_geometry_msgs.do_transform_pose(pose, trans) self.pose_transformed = transformed.pose print self.pose_transformed try: self.omni_base.go_rel(0.0, self.pose_transformed.position.y, 0.0, 10.0) except: print("Time out")
def goal_callback(self, msg): rospy.logdebug("Received goal pose: %s", str(msg)) try: if self.simulation: # Simulated trans = self.tf_buffer.lookup_transform('map', 'mechROS_base_link', rospy.Time(0), rospy.Duration(1.0)) pose_transformed = tf2_geometry_msgs.do_transform_pose(msg, trans) pose_transformed.header.frame_id = 'map' self.target_pose = pose_transformed else: # Real robot self.target_pose = msg self.signal.set() except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.loginfo('Could not find transform')
def execute(self, ud): rospy.logdebug("Entered 'SET_OBJECT_TARGET' state.") new_target = '' is_new_obj = False # loop through all found objects for i in range(len(ud.sot_object_array)): # get object info, mainly its name try: req = GetObjectInformationRequest() req.type = ud.sot_object_array[i].type res = self.ork_srv(req) # if the object hasn't been grasped yet, make the object the current grasp target is_new_obj = True if any(res.information.name in s for s in ud.sot_picked_objects): is_new_obj = False if is_new_obj: new_target = res.information.name except rospy.ServiceException: rospy.logerr("GetObjectInformation service request failed.") # if a new object has been detected if new_target and is_new_obj: # get the transform from ork's frame to odom transform = self.tf_buffer.lookup_transform('odom', ud.sot_ork_frame, rospy.Time(0)) # get the object's pose in odom frame ud.sot_grasp_target_pose = do_transform_pose(ud.sot_object_array[i].pose.pose, transform) ud.sot_target_object = new_target ud.sot_grasp_target_pose.pose.position.z += 0.08 ud.sot_grasp_target_pose.pose.position.x -= 0.15 print "OBJECT NAME : " + new_target print "POSE : " print ud.sot_grasp_target_pose check_call([PATH_TO_PDF_CREATOR], shell=True) check_call(['~/sara_ws/src/PDF_creator/create_pdf.sh'], shell=True) return 'target_set' # rospy.logerr("Could not get transform from " + ud.sot_ork_frame + " to 'odom'.") return 'no_new_object'
def check_direction(): global listener, tfBuffer, cj_injection_message speed = 0.1 # default speed m/s rot_speed = 0.5 # default rot speed sec/radian min_duration = 2.0 # minimum time [sec] for single trajectory duration = default_duration = 2 # sec trans = None try: trans = tfBuffer.lookup_transform(prefix, 'world', rospy.Time(0)) except: rospy.logwarn("tf lookup -- {} not found".format(prefix)) if trans != None: cj_local_coord = PoseStamped() cj_local_coord = tf2_geometry_msgs.do_transform_pose(cj_injection_message, trans) # rospy.loginfo(cj_local_coord) heading = atan2(cj_local_coord.pose.position.y, cj_local_coord.pose.position.x) # rospy.loginfo(heading) distance = sqrt(pow(cj_local_coord.pose.position.x, 2) + pow(cj_local_coord.pose.position.y, 2)) duration = distance / speed # #calculate required time for this motion # rospy.logdebug("in check_direction: before if: [{},{}]".format(heading, duration)) if duration < min_duration: duration = min_duration q = (cj_local_coord.pose.orientation.x, cj_local_coord.pose.orientation.y, cj_local_coord.pose.orientation.z, cj_local_coord.pose.orientation.w) euler = euler_from_quaternion(q, axes='sxyz') rotation_yaw = abs(euler[2]) duration_yaw = rotation_yaw / rot_speed # rospy.logdebug("in check_direction: duration_yaw: {}".format(duration_yaw)) # rospy.logdebug("in check_direction: duration: {}".format(duration)) if duration_yaw > duration: duration = duration_yaw # rospy.logdebug("in check_direction. returning: [{},{}]".format(heading, duration)) return [heading, duration] else: rospy.logerr("in check_direction: transform is None") return [0, duration]
def my_callback(event): try: transf = tfBuffer.lookup_transform("map", BASE_FRAME, rospy.Time(), timeout=rospy.Duration(2)) rotVec = PoseStamped() rotVec.header.frame_id = "map" rotVec.pose.orientation.w = 1.0 rotVec = tf2_geometry_msgs.do_transform_pose(rotVec, transf) quaternion = [ rotVec.pose.orientation.x, rotVec.pose.orientation.y, rotVec.pose.orientation.z, rotVec.pose.orientation.w ] euler = tf.transformations.euler_from_quaternion(quaternion) yaw = euler[2] estimated_orientation.z = yaw travel_x = last_cmdvel.x * UPDATE_PERIOD travel_y = last_cmdvel.y * UPDATE_PERIOD travel_x = (travel_x * cos(estimated_orientation.z) + travel_y * sin(estimated_orientation.z)) travel_y = (travel_x * sin(estimated_orientation.z) + travel_y * cos(estimated_orientation.z)) estimated_position.x = estimated_position.x + travel_x / 2 estimated_position.y = estimated_position.y + travel_y / 2 omega = last_cmdvel.z * UPDATE_PERIOD if not has_odom: estimated_orientation.z += omega odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = ODOM_FRAME odom_quat = tf.transformations.quaternion_from_euler( estimated_orientation.z, 0, 0, "rzyx") odom.pose.pose = Pose( Point(estimated_position.x, estimated_position.y, 0), Quaternion(*odom_quat)) odom.child_frame_id = BASE_FRAME odom.twist.twist = Twist(Vector3(travel_x, travel_y, 0), Vector3(0, 0, omega)) odom_pub.publish(odom) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: print(ex) pass
def get_detector_results(self, request): """ Args: request (GetDetectorResultsRequest): Returns: GetDetectorResultsResponse """ if self.currently_busy.is_set(): return GetDetectorResultsResponse(status=ServiceStatus(BUSY=True)) self.currently_busy.set() detections = Detections() try: rois = [] poses = [] for pose in self.obj_poses: transform = self.tf_buffer.lookup_transform(self.ref_frame, 'odom', rospy.Time()) pose = tf2_geometry_msgs.do_transform_pose(pose, transform).pose x1 = pose.position.x - 0.03 x2 = pose.position.x + 0.03 y1 = pose.position.y - 0.03 y2 = pose.position.y + 0.03 z1 = pose.position.z - 0.03 z2 = pose.position.z + 0.03 roi = RegionOfInterest(x1=x1, y1=y1, z1=z1, x2=x2, y2=y2, z2=z2) rois.append(roi) poses.append(pose) self.currently_busy.clear() # Fill in the detections message for roi, pose, track_id in zip(rois, poses, self.obj_ids): seg_roi = SegmentOfInterest(x=[], y=[]) score = 1.0 detections.objects.append(Detection(roi=roi, seg_roi=seg_roi, pose=pose, id=self._new_id(), track_id=track_id, confidence=score, class_name=self.classes[0])) return GetDetectorResultsResponse(status=ServiceStatus(OKAY=True), results=detections) except Exception as e: print("GazeboRenderedBerriesServer error: ", e) return GetDetectorResultsResponse(status=ServiceStatus(ERROR=True), results=detections)
def _odom_callback(self, msg): with self._lock: try: transform = self.tf_buffer.lookup_transform( 'base_link', msg.child_frame_id, rospy.Time.now(), rospy.Duration(1.0)) except Exception as e: rospy.logerr('ERROR in TransformOdomNode: ' + str(e)) raise stamped_point = PoseStamped() stamped_point.pose = msg.pose.pose final_pose = tf2_geometry_msgs.do_transform_pose( stamped_point, transform) msg_final = self._construct_transform(final_pose, msg) self._transform_broadcaster.sendTransform(msg_final)
def handle_req_s_tr(self,req): resp = LookAtTSResponse() try: # Get position from the request pos = req.desired_s_tr.transform.translation # Lookup the transform between frames trans = self.tfBuffer.lookup_transform(self.base, req.desired_s_tr.child_frame_id, rospy.Time(0)) # Apply the transform to the requested position pose = PoseStamped() pose.pose.position = Point(pos.x, pos.y, pos.z) pose.pose.orientation = req.desired_s_tr.transform.rotation trans = tf2_geometry_msgs.do_transform_pose(pose, trans).pose.position # Send off the request resp.success = self.lookat(trans) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): print 'Cannot lookupTransform' resp.success = False return resp
def __init__(self, from_frame, to_frame): pose_stamped = PoseStamped() pose_stamped.header.frame_id = from_frame pose_stamped.header.stamp = rospy.Time.now() tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) # tf buffer length tf_listener = tf2_ros.TransformListener(tf_buffer) while not rospy.is_shutdown(): try: rospy.sleep(0.1) transform = tf_buffer.lookup_transform(to_frame, from_frame, # source frame rospy.Time(0), # get the tf at first available time rospy.Duration(0.1)) # wait for 1 second pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform) pose_pub.publish(pose_transformed) except Exception as e: rospy.logwarn(e) continue
def execute(self, ud): loop_again = True while loop_again: self.mutex.acquire() if self.msg_received: loop_again = False self.mutex.release() rospy.sleep(rospy.Duration(1)) transform = self.tf_buffer.lookup_transform('odom', self.tables.header.frame_id, rospy.Time(0)) drop_pose_z = 1.5 tmp = PoseStamped() lst = [] for t in range(len(self.tables.tables)): tmp.pose = self.tables.tables[t].pose tmp.header.frame_id = self.tables.header.frame_id tmp.header.stamp = rospy.Time.now() eval_pose = do_transform_pose(tmp, transform) rpy = tf_conversions.transformations.euler_from_quaternion(eval_pose.pose.orientation) if rpy[2]**2 > 0.90: lst.append([(eval_pose.pose.position.z - drop_pose_z)**2, eval_pose]) lst.sort() ud.drop_pose = lst[0][1] print ud.drop_pose return 'drop_pose_acquired'
def do_playback_keyframe_demo(self, goal): rospy.loginfo("Received playback demo") ## input: keyframe demo goal has bagfile name and eef_only bool self.start_time = time.time() tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) tfBroadcaster = tf2_ros.TransformBroadcaster() # Check if the bag file is valid # Example path if necessary bag_path = os.path.expanduser(goal.bag_file_name) if (not os.path.isfile(bag_path)): error_msg = "Playback bagfile does not exist: %s" % bag_path self.server.set_aborted(self.result, error_msg) rospy.logerr(error_msg) return with rosbag.Bag(bag_path) as bag: self.bag = bag # Check the number of playback - and warn/stop if we have more than X number of keyframes if self.bag.get_message_count() > self.KEYFRAME_THRESHOLD: error_msg = "Playback Keyframe Demo aborted due to too many frames: %d" % self.bag.get_message_count() self.server.set_aborted(self.result, error_msg) rospy.logerr(error_msg) return keyframe_count = 0 # Check if we have a gripper topic. If so add it to playback all_topics = self.bag.get_type_and_topic_info().topics.keys() playback_topics = [goal.target_topic] GRIPPER_TOPIC = 'gripper/stat' gripper_topics = [x for x in all_topics if GRIPPER_TOPIC in x] playback_topics.extend(gripper_topics) OBJECT_LOCATION_TOPIC = "object_location" playback_topics.append(OBJECT_LOCATION_TOPIC) gripper_msgs = dict() self.gripper_pos = dict() while not self.server.is_preempt_requested(): # Cycle through the topics and messages and store them into list for ordering all_messages = dict() for topic, msg, t in self.bag.read_messages(topics=playback_topics): if topic not in all_messages: all_messages[topic] = [] all_messages[topic].append(msg) # Pull out the playback topic playback_list = all_messages[goal.target_topic] for gripper_topic in gripper_topics: # either one or two grippers gripper_msgs[gripper_topic] = all_messages[gripper_topic] # Actually set the gripper value pos = gripper_msgs[gripper_topic][0].requested_position self.gripper_helper(gripper_topic, pos) for msg_count in xrange(len(playback_list)): msg = playback_list[msg_count] if goal.zero_marker: zeroMarker = None if not OBJECT_LOCATION_TOPIC in all_messages: rospy.logerr("Playback specified a zero marker but no object locations were found in keyframe #{}".format(msg_count)) self.server.set_aborted(text="Object locations missing") return for i, location in enumerate(all_messages[OBJECT_LOCATION_TOPIC][msg_count].objects): if location.label == goal.zero_marker: zeroMarker = location break if not zeroMarker: rospy.logerr("Specified zero marker not found in .bag file in keyframe #{}".format(msg_count)) self.server.set_aborted(text="Zero marker not found in .bag file") return try: currentZero = tfBuffer.lookup_transform("map", goal.zero_marker, rospy.Time(0), timeout=rospy.Duration(5)).transform except tf2_ros.LookupException: rospy.logerr("Specified label \"{}\" not found in current scene. Disable locate objects to play back absolute keyframe positions.".format(zeroMarker.label)) self.server.set_aborted(text="Specified label not found") return rospy.loginfo("Using zero marker \"{}\" (prob: {:.1%}) with position (x: {:.2f}, y: {:.2f}, z: {:.2f})".format( zeroMarker.label, zeroMarker.probability, zeroMarker.pose.position.x, zeroMarker.pose.position.y, zeroMarker.pose.position.z )) rospy.loginfo("Found corresponding zero marker in current frame (x: {:.2f}, y: {:.2f}, z: {:.2f})".format( currentZero.translation.x, currentZero.translation.y, currentZero.translation.z )) else: rospy.loginfo("No zero marker passed for keyframe #{}".format(msg_count)) # Check if we need to convert the msg into joint values if goal.eef_only: # Ask the arm planner to plan for that joint target from current position pt = Pose(msg.position, msg.orientation) if zeroMarker: # Remap the point to the global map frame for adjustment baseLinkToMap = tfBuffer.lookup_transform("map", "base_link", rospy.Time(0), timeout=rospy.Duration(5)) mapToBaseLink = tfBuffer.lookup_transform("base_link", "map", rospy.Time(0), timeout=rospy.Duration(5)) ptAbsolute = tf2_geometry_msgs.do_transform_pose(PoseStamped(pose=pt), baseLinkToMap) # Compensate for new zero marker position ptAbsolute.pose.position.x += (currentZero.translation.x - zeroMarker.pose.position.x) ptAbsolute.pose.position.y += (currentZero.translation.y - zeroMarker.pose.position.y) ptAbsolute.pose.position.z += (currentZero.translation.z - zeroMarker.pose.position.z) pt = tf2_geometry_msgs.do_transform_pose(ptAbsolute, mapToBaseLink).pose rospy.loginfo("Moving EEF to adjusted position: (x: {:.2f}, y: {:.2f}, z: {:.2f})".format( pt.position.x, pt.position.y, pt.position.z )) plan = self.arm_planner.plan_poseTargetInput(pt) else: # Pull out the joint values for the arm from the message pts = self._get_arm_joint_values(msg) # Ask the arm planner to plan for that joint target from current position plan = self.arm_planner.plan_jointTargetInput(pts) # Check if we have a valid plan if plan == None or len(plan.joint_trajectory.points) < 1: print "Error: no plan found" else: rospy.loginfo("Executing Keyframe: %d" % keyframe_count) self.sendPlan(plan) keyframe_count+=1 # Execute Gripper if needed for gripper_topic in gripper_topics: pos = gripper_msgs[gripper_topic][msg_count].requested_position if abs(pos - self.gripper_pos[gripper_topic]) > self.GRIPPER_THRESHOLD: # Actually set the gripper value self.gripper_helper(gripper_topic, pos) self.result.time_elapsed = rospy.Duration.from_sec(time.time() - self.start_time) complete_msg = "Playback Keyframe Demo completed successfully" self.server.set_succeeded(self.result, complete_msg) rospy.loginfo(complete_msg) return self.result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) self.server.set_preempted(self.result, "playback keyframe demo preempted") print 'preempted' return
def pick_and_place_aruco(self, string_operation): success = False if string_operation == "pick": self.prepare_robot_pandp() rospy.sleep(2.0) while not rospy.is_shutdown() and self.aruco_pose_rcv == False: rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection...") rospy.sleep(1.0) aruco_pose = self.aruco_pose aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id) rospy.loginfo("Got: " + str(aruco_pose)) rospy.loginfo("spherical_grasp_gui: Transforming from frame: " + aruco_pose.header.frame_id + " to 'base_footprint'") ps = PoseStamped() ps.pose.position = aruco_pose.pose.position ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) ps.header.frame_id = aruco_pose.header.frame_id transform_ok = False while not transform_ok and not rospy.is_shutdown(): try: transform = self.tfBuffer.lookup_transform("base_footprint", ps.header.frame_id, rospy.Time(0)) aruco_ps = do_transform_pose(ps, transform) transform_ok = True except tf2_ros.ExtrapolationException as e: rospy.logwarn( "Exception on transforming point... trying again \n(" + str(e) + ")") rospy.sleep(0.01) ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id) rospy.loginfo("Setting cube pose based on Aruco detection") self.pick_g.object_pose.pose.position = aruco_ps.pose.position self.pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0) rospy.loginfo("aruco pose in base_footprint:" + str(self.pick_g)) self.pick_g.object_pose.header.frame_id = 'base_footprint' self.pick_g.object_pose.pose.orientation.w = 1.0 self.detected_pose_pub.publish(self.pick_g.object_pose) rospy.loginfo("Gonna pick:" + str(self.pick_g)) self.pick_as.send_goal_and_wait(self.pick_g) rospy.loginfo("Done!") result = self.pick_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to pick, not trying further") success = False else: success = True self.prepare_robot_nav() return success if string_operation == "place": # Place the object on table in front rospy.loginfo("Placing aruco marker") self.place_as.send_goal_and_wait(self.pick_g) rospy.loginfo("Done!") result = self.place_as.get_result() if str(moveit_error_dict[result.error_code]) != "SUCCESS": rospy.logerr("Failed to place, not trying further") success = False else: success = True return success