def save_dynamic_tf(bag, kitti_type, kitti, initial_time): print("Exporting time dependent transformations") if kitti_type.find("raw") != -1: for timestamp, oxts in zip(kitti.timestamps, kitti.oxts): tf_oxts_msg = TFMessage() tf_oxts_transform = TransformStamped() tf_oxts_transform.header.stamp = rospy.Time.from_sec(float(timestamp.strftime("%s.%f"))) tf_oxts_transform.header.frame_id = 'world' tf_oxts_transform.child_frame_id = 'base_link' transform = (oxts.T_w_imu) T_imu_to_base_link = np.eye(4, 4) T_imu_to_base_link[0:3, 3] = [2.71/2.0+0.05, -0.32, -0.93] transform = np.dot(transform,T_imu_to_base_link) t = transform[0:3, 3] q = tf.transformations.quaternion_from_matrix(transform) oxts_tf = Transform() oxts_tf.translation.x = t[0] oxts_tf.translation.y = t[1] oxts_tf.translation.z = t[2] oxts_tf.rotation.x = q[0] oxts_tf.rotation.y = q[1] oxts_tf.rotation.z = q[2] oxts_tf.rotation.w = q[3] tf_oxts_transform.transform = oxts_tf tf_oxts_msg.transforms.append(tf_oxts_transform) bag.write('/tf', tf_oxts_msg, tf_oxts_msg.transforms[0].header.stamp) elif kitti_type.find("odom") != -1: timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) for timestamp, tf_matrix in zip(timestamps, kitti.poses): tf_msg = TFMessage() tf_stamped = TransformStamped() tf_stamped.header.stamp = rospy.Time.from_sec(timestamp) tf_stamped.header.frame_id = 'camera_init' tf_stamped.child_frame_id = 'camera_gray_left' t = tf_matrix[0:3, 3] q = tf.transformations.quaternion_from_matrix(tf_matrix) transform = Transform() transform.translation.x = t[0] transform.translation.y = t[1] transform.translation.z = t[2] transform.rotation.x = q[0] transform.rotation.y = q[1] transform.rotation.z = q[2] transform.rotation.w = q[3] tf_stamped.transform = transform tf_msg.transforms.append(tf_stamped) bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def setup_forearm_twenty_two_taxels_transforms(self): self.link_name_forearm = '/%s_forearm_link'%(self.arm) n_taxels = 22 self.tar_forearm.data = [Transform()] * n_taxels # [Transform() for i in range(n_taxels)] #[None for i in range(n_taxels)] idx_list = [14, 10, 16, 8, 9, 12, 0, 1, 4, 19, 21, 15, 7, 13, 2, 3, 6, 5, 20, 17, 11, 18] x_disp = [.16, .23, .3, .16, .23, .3, .16, .23, .3, .16, .23, .3, .17, .28, .17, .28, .17, .28, .17, .28, .34, .34] y_disp = [0., 0., 0., -0.06, -0.06, -0.06, 0., 0., 0., 0.06, 0.06, 0.06, -0.05, -0.05, -0.05, -0.05, 0.05, 0.05, 0.05, 0.05 ,-0.05, 0.05] z_disp = [0.04, 0.02, 0.03, 0., 0., 0., -0.05, -0.05, -0.05, 0., 0., 0., 0.04, 0.02, -0.04, -0.04, -0.04, -0.04, 0.04, 0.02, 0., 0.] x_ang = [0, 0, 0, -math.pi/2, -math.pi/2, -math.pi/2, math.pi, math.pi, math.pi, math.pi/2, math.pi/2, math.pi/2, -math.pi/4, -math.pi/4, -3*math.pi/4, -3*math.pi/4, 3*math.pi/4, 3*math.pi/4, math.pi/4, math.pi/4, math.radians(-30), math.radians(30)] y_ang = [-math.pi/4, math.radians(-15), math.radians(20), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, math.radians(-60), math.radians(-60)] z_ang = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] for i in range(n_taxels): t = Transform() t.translation.x = x_disp[i] t.translation.y = y_disp[i] t.translation.z = z_disp[i] rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i]) quat = tr.matrix_to_quaternion(rot_mat) t.rotation.x = quat[0] t.rotation.y = quat[1] t.rotation.z = quat[2] t.rotation.w = quat[3] self.tar_forearm.data[idx_list[i]] = t
def publish_transforms(self, ts): ego_t = Transform() ego_t.translation.x = self.ego_pose[0] ego_t.translation.y = self.ego_pose[1] ego_t.translation.z = 0.0 ego_quat = quaternion_from_euler(0.0, 0.0, self.ego_pose[2]) ego_t.rotation.x = ego_quat[0] ego_t.rotation.y = ego_quat[1] ego_t.rotation.z = ego_quat[2] ego_t.rotation.w = ego_quat[3] ego_ts = TransformStamped() ego_ts.transform = ego_t ego_ts.header.stamp = ts ego_ts.header.frame_id = '/map' ego_ts.child_frame_id = 'ego_racecar/base_link' opp_t = Transform() opp_t.translation.x = self.opp_pose[0] opp_t.translation.y = self.opp_pose[1] opp_t.translation.z = 0.0 opp_quat = quaternion_from_euler(0.0, 0.0, self.opp_pose[2]) opp_t.rotation.x = opp_quat[0] opp_t.rotation.y = opp_quat[1] opp_t.rotation.z = opp_quat[2] opp_t.rotation.w = opp_quat[3] opp_ts = TransformStamped() opp_ts.transform = opp_t opp_ts.header.stamp = ts opp_ts.header.frame_id = '/map' opp_ts.child_frame_id = 'opp_racecar/base_link' self.br.sendTransform(ego_ts) self.br.sendTransform(opp_ts)
def timer_callback(self, event): if self.last_recieved_stamp is None: return cmd = Odometry() cmd.header.stamp = self.last_recieved_stamp cmd.header.frame_id = 'map' cmd.child_frame_id = 'base_link' cmd.pose.pose = self.last_received_pose cmd.twist.twist = self.last_received_twist self.pub_odom.publish(cmd) tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id, stamp=cmd.header.stamp), child_frame_id=cmd.child_frame_id, transform=Transform( translation=cmd.pose.pose.position, rotation=cmd.pose.pose.orientation)) self.tf_pub.sendTransform(tf) tf = TransformStamped(header=Header(frame_id='map', stamp=self.last_recieved_stamp), child_frame_id='rear_right_wheel', transform=Transform( translation=self.right_wheel_pose.position, rotation=self.right_wheel_pose.orientation)) self.tf_pub.sendTransform(tf) tf = TransformStamped(header=Header(frame_id='map', stamp=self.last_recieved_stamp), child_frame_id='rear_left_wheel', transform=Transform( translation=self.left_wheel_pose.position, rotation=self.left_wheel_pose.orientation)) self.tf_pub.sendTransform(tf)
def read_samples(filepath): samples = [] with open(filepath, 'r') as inputfile: reader = csv.reader(inputfile) header = next(reader) for line in reader: # transform from robot base to end efector rob = Transform() rob.translation.x = float(line[0]) rob.translation.y = float(line[1]) rob.translation.z = float(line[2]) rob.rotation.x = float(line[3]) rob.rotation.y = float(line[4]) rob.rotation.z = float(line[5]) rob.rotation.w = float(line[6]) # transform from camera to marker opt = Transform() opt.translation.x = float(line[7]) opt.translation.y = float(line[8]) opt.translation.z = float(line[9]) opt.rotation.x = float(line[10]) opt.rotation.y = float(line[11]) opt.rotation.z = float(line[12]) opt.rotation.w = float(line[13]) samples.append({'robot': rob, 'optical': opt}) return samples
def publish_waypoint(x, y, z, yaw): """ Publish a waypoint to """ command_publisher = rospy.Publisher('/iris/command/trajectory', MultiDOFJointTrajectory, queue_size=10) # create trajectory msg traj = MultiDOFJointTrajectory() traj.header.stamp = rospy.Time.now() traj.header.frame_id = 'frame' traj.joint_names.append('base_link') # create start point for trajectory transforms = Transform() transforms.translation.x = 0 transforms.translation.y = 0 transforms.translation.z = 0 quat = tf.transformations.quaternion_from_euler(yaw * np.pi / 180.0, 0, 0, axes='rzyx') transforms.rotation.x = quat[0] transforms.rotation.y = quat[1] transforms.rotation.z = quat[2] transforms.rotation.w = quat[3] velocities = Twist() accel = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel], rospy.Time(2)) traj.points.append(point) # create end point for trajectory transforms = Transform() transforms.translation.x = x transforms.translation.y = y transforms.translation.z = z quat = tf.transformations.quaternion_from_euler((yaw) * np.pi / 180.0, 0, 0, axes='rzyx') transforms.rotation.x = quat[0] transforms.rotation.y = quat[1] transforms.rotation.z = quat[2] transforms.rotation.w = quat[3] velocities = Twist() accel = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel], rospy.Time(2)) traj.points.append(point) rospy.sleep(1) command_publisher.publish(traj)
def callback(scan): try: t, r = tf_listener.lookupTransform(topics.WORLD_FRAME, topics.ODOMETRY_FRAME, rospy.Time(0)) transform = Transform(translation=Vector3(*t), rotation=Quaternion(*r)) except Exception as e: rospy.logerr('Failed to lookup transform') transform = Transform(translation=Vector3(0, 0, 0), rotation=Quaternion(0, 0, 0, 1)) map_pub.update( MapUpdate(scan=process_msg(unpickle(scan)), transform=transform))
def test_april_tags_single_interval(self): #Setup the publisher and subscribers self.pub_april_tags = rospy.Publisher( "visual_odometry_april_tags_node/april_tags", AprilTags, queue_size=1, latch=True) self.pub_wheels_cmd = rospy.Publisher( "visual_odometry_april_tags_node/wheels_cmd", WheelsCmdStamped, queue_size=1, latch=True) # Wait for the forward_kinematics_node to finish starting up timeout = time.time() + 5.0 while (self.pub_wheels_cmd.get_num_connections() < 1 or self.pub_april_tags.get_num_connections() < 1) and \ not rospy.is_shutdown() and not time.time()>timeout: rospy.sleep(0.1) # Publish a single wheels cmd, and two simple april tag messages msg_wheels_cmd = WheelsCmdStamped() msg_wheels_cmd.vel_left = 1 msg_wheels_cmd.vel_right = 1 self.pub_wheels_cmd.publish(msg_wheels_cmd) rospy.sleep(0.2) #Wait so the tags come in the right order T1 = Transform() T2 = Transform() T1.translation.y = 2 T2.translation.y = 2 T2.rotation.x, T2.rotation.y, T2.rotation.z, T2.rotation.w = tr.quaternion_about_axis( -np.pi / 2, (0, 0, 1)) msg_tag1 = AprilTags() tag = TagDetection() tag.transform = T1 msg_tag1.detections.append(tag) msg_tag1.header.stamp = rospy.Duration(0) self.pub_april_tags.publish(msg_tag1) rospy.sleep(0.2) #Wait so the tags come in the right order msg_tag2 = AprilTags() msg_tag1.header.stamp = rospy.Duration(1) tag.transform = T2 msg_tag1.detections.append(tag) self.pub_april_tags.publish(msg_tag1) # Wait 1 second for the file to be output rospy.sleep(3) res = np.genfromtxt( rospy.get_param("visual_odometry_april_tags_node/filename", '')) assert_almost_equal(res, np.array([1, 1, 1, np.pi / 2, 2, 2]))
def save_dynamic_tf(bag, timestamps, tf_matrices, child_frame_id): print("Exporting time dependent transformations") for timestamp, tf_matrix in zip(timestamps, tf_matrices): tf_msg = TFMessage() tf_stamped = TransformStamped() tf_stamped.header.stamp = to_rostime(timestamp) tf_stamped.header.frame_id = 'world' tf_stamped.child_frame_id = child_frame_id t = tf_matrix[0:3, 3] q = quaternion_from_matrix(tf_matrix) transform = Transform() transform.translation.x = t[0] transform.translation.y = t[1] transform.translation.z = t[2] transform.rotation.x = q[0] transform.rotation.y = q[1] transform.rotation.z = q[2] transform.rotation.w = q[3] tf_stamped.transform = transform tf_msg.transforms.append(tf_stamped) bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp)
def fromRT(rt): if (rt[0, 0] + rt[1, 1] + rt[2, 2] > 0): s = math.sqrt(1.0 + rt[0, 0] + rt[1, 1] + rt[2, 2]) * 2 #s=qw*4 qw = s / 4 qx = (rt[2, 1] - rt[1, 2]) / s qy = (rt[0, 2] - rt[2, 0]) / s qz = (rt[1, 0] - rt[0, 1]) / s elif ((rt[0, 0] > rt[1, 1]) and (rt[0, 0] > rt[2, 2])): s = math.sqrt(1.0 + rt[0, 0] - rt[1, 1] - rt[2, 2]) * 2 #s=qx*4 qw = (rt[2, 1] - rt[1, 2]) / s qx = s / 4 qy = (rt[0, 1] + rt[1, 0]) / s qz = (rt[0, 2] + rt[2, 0]) / s elif (rt[1, 1] > rt[2, 2]): s = math.sqrt(1.0 - rt[0, 0] + rt[1, 1] - rt[2, 2]) * 2 #s=qy*4 qw = (rt[0, 2] - rt[2, 0]) / s qx = (rt[0, 1] + rt[1, 0]) / s qy = s / 4 qz = (rt[1, 2] + rt[2, 1]) / s else: s = math.sqrt(1.0 - rt[0, 0] - rt[1, 1] + rt[2, 2]) * 2 #s=qz*4 qw = (rt[1, 0] - rt[0, 1]) / s qx = (rt[0, 2] + rt[2, 0]) / s qy = (rt[1, 2] + rt[2, 1]) / s qz = s / 4 tf = Transform() tf.rotation.w = qw tf.rotation.x = qx tf.rotation.y = qy tf.rotation.z = qz tf.translation.x = rt[0, 3] tf.translation.y = rt[1, 3] tf.translation.z = rt[2, 3] return tf
def publish_pose_speed_command(self, x, y, z, rx, ry, rz, speed_vector): print('Command pose: '+self._namespace, x, y, z, rx, ry, rz, speed_vector ) quaternion = tf.transformations.quaternion_from_euler(rx, ry, rz) rotation = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]) location = Point(x, y, z) transforms = Transform(translation=location, rotation=rotation) velocities = Twist() velocities.linear = speed_vector accelerations = Twist() traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(0)) traj.points.append(point) self.command_pub.publish(traj)
def collisionCallback(self, feedback): ''' This callback runs on every feedback message received ''' validity_msg = GetStateValidityRequest( ) # Build message to verify collision validity_msg.group_name = PLANNING_GROUP if self.next_pose and (not self.collision): self.odometry_me.acquire() x = self.odometry.position.x y = self.odometry.position.y z = self.odometry.position.z # Distance between the robot and the next position dist = sqrt((self.next_pose.position.x - x)**2 + (self.next_pose.position.y - y)**2 + (self.next_pose.position.z - z)**2) # Pose to verify collision pose = Transform() pose.rotation.x = self.odometry.orientation.x pose.rotation.y = self.odometry.orientation.y pose.rotation.z = self.odometry.orientation.z pose.rotation.w = self.odometry.orientation.w self.odometry_me.release() #Verify possible collisions on diferent points between the robot and the goal point # rospy.logerr("\n\n\nCOLLISION CALLBACK: ") # rospy.logerr(dist) for d in arange(RESOLUTION, dist + 0.5, RESOLUTION): pose.translation.x = (self.next_pose.position.x - x) * (d / dist) + x pose.translation.y = (self.next_pose.position.y - y) * (d / dist) + y pose.translation.z = (self.next_pose.position.z - z) * (d / dist) + z self.current_pose.multi_dof_joint_state.transforms[ 0] = pose # Insert the correct odometry value validity_msg.robot_state = self.current_pose # Call service to verify collision collision_res = self.validity_srv.call(validity_msg) # print("\nCollision response:") # print(collision_res) # Check if robot is in collision if not collision_res.valid: # print(validity_msg) rospy.logerr('Collision in front [x:{} y:{} z:{}]'.format( pose.translation.x, pose.translation.y, pose.translation.z)) rospy.logerr('Current pose [x:{} y:{} z:{}]'.format( x, y, z)) # print(collision_res) self.move_client.cancel_goal() self.collision = True return
def timer_callback(self, event): if self.last_recieved_stamp is None: return if self.last_recieved_stamp == self.last_stamp: return #TODO self.last_stamp = self.last_recieved_stamp cmd = Odometry() cmd.header.stamp = self.last_recieved_stamp # print(cmd.header.stamp) cmd.header.frame_id = 'odom' # this used to be map # I set this to be 'odom' cmd.child_frame_id = 'base_footprint' #I set to be base_link #BeiYou set to be base_link #this used to be odom cmd.pose.pose = self.last_received_pose cmd.twist.twist = self.last_received_twist self.pub_odom.publish(cmd) tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id, stamp=cmd.header.stamp), child_frame_id=cmd.child_frame_id, transform=Transform( translation=cmd.pose.pose.position, rotation=cmd.pose.pose.orientation)) self.tf_pub.sendTransform(tf)
def __init__(self, pose=TransformStamped( header=Header(frame_id='panda_link8'), child_frame_id='target', transform=Transform(rotation=Quaternion( *tf.quaternion_about_axis(numpy.pi / 4, [0, 0, 1])), translation=Vector3(0, 0, 0.105)))): self.robot = RobotModel() self.robot._add(Joint(pose)) # add a fixed end-effector transform self.pub = rospy.Publisher('/target_joint_states', JointState, queue_size=10) self.joint_msg = JointState() self.joint_msg.name = [j.name for j in self.robot.active_joints] self.joint_msg.position = numpy.asarray([ (j.min + j.max) / 2 + 0.1 * (j.max - j.min) * random.uniform(0, 1) for j in self.robot.active_joints ]) self.target_link = pose.child_frame_id self.T, self.J = self.robot.fk( self.target_link, dict(zip(self.joint_msg.name, self.joint_msg.position))) self.im_server = MyInteractiveMarkerServer("controller", self.T)
def inverse_transform(transform): """ Get the inverse of a tf transform Get frame 2 -> frame 1 from frame 1 -> frame 2 :param transform: Transform from frame 1 -> frame 2 :type transform: geometry_msgs/Transform :return: Transform from frame 2 -> frame 1 :rtype: geometry_msgs/Transform """ tmat = translation_matrix( (transform.translation.x, transform.translation.y, transform.translation.z)) qmat = quaternion_matrix((transform.rotation.x, transform.rotation.y, transform.rotation.z, transform.rotation.w)) tf_mat = np.dot(tmat, qmat) inv_tf_mat = inverse_matrix(tf_mat) inv_transform = Transform() inv_transform.translation.x = translation_from_matrix(inv_tf_mat)[0] inv_transform.translation.y = translation_from_matrix(inv_tf_mat)[1] inv_transform.translation.z = translation_from_matrix(inv_tf_mat)[2] inv_transform.rotation = Quaternion(*quaternion_from_matrix(inv_tf_mat)) return inv_transform
def get_current_ros_transform(self): """ Override function used to return the current ROS transform of the object The global map frame has an empty transform. :return: """ return Transform()
def publish_command(position,velocity): rospy.init_node('goto_poition',anonymous=True) firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory',MultiDOFJointTrajectory,queue_size=10) desired_yaw = -10 desired_x = position[0] desired_y = position[1] desired_z = position[2] quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw)) traj = MultiDOFJointTrajectory() header = std_msgs.msg.Header() header.stamp = rospy.Time() header.frame_id = 'frame' traj.joint_names.append('base_link') traj.header = header transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3])) velocities = Twist() velocities.linear.x = velocity[0] velocities.linear.y = velocity[1] velocities.linear.z = velocity[2] accelerations = Twist() point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2)) traj.points.append(point) time.sleep(3) firefly_command_publisher.publish(traj) rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def __init__(self, calibration_parameters=None, transformation=None): """ Creates a HandeyeCalibration object. :param transformation: transformation between optical origin and base/tool robot frame as tf tuple :type transformation: ((float, float, float), (float, float, float, float)) :return: a HandeyeCalibration object :rtype: easy_handeye.handeye_calibration.HandeyeCalibration """ self.parameters = calibration_parameters if transformation is None: transformation = ((0, 0, 0), (0, 0, 0, 1)) self.transformation = TransformStamped(transform=Transform( Vector3(*transformation[0]), Quaternion(*transformation[1]))) """ transformation between optical origin and base/tool robot frame :type: geometry_msgs.msg.TransformedStamped """ # tf names if self.parameters.eye_on_hand: self.transformation.header.frame_id = calibration_parameters.robot_effector_frame else: self.transformation.header.frame_id = calibration_parameters.robot_base_frame self.transformation.child_frame_id = calibration_parameters.tracking_base_frame
def __init__(self, name): """! The init function instantiates the class and loads all of the relevant information from the parameter server. @param nameThe name of this robot. Also includes how to find information about it on the parameter server. """ # Set the name self._name = name # Look up the class number and associated ID self._class_id = ParameterLookup.lookup( '~' + self._resolveString('class')) self._id = ParameterLookup.lookup('~' + self._resolveString("id")) # Also look up the keypoints used for this robot self._keypoints = self._initializeKeypoints( '~' + self._resolveString("keypoints")) # Look up the bounding shape, which is treated mathematically # the same as keypoints. self._bounding_shape = self._initializeKeypoints( '~' + self._resolveString('bounding_shape')) # Determine what name to use to find the robot in the TF tree. self._frame_id = ParameterLookup.lookupWithDefault( '~' + self._resolveString("frame_id"), "base_link") # Set the pose to the origin. transform = Transform() transform.rotation.w = 1.0 self.recordTransform(transform)
def to_Transform(klampt_se3): """From Klampt se3 element to ROS Transform """ ros_pose = Transform() ros_pose.orientation = to_Quaternion(klampt_se3[0]) ros_pose.translation.x, ros_pose.translation.y, ros_pose.translation.z = klampt_se3[ 1] return ros_pose
def setUp(self): self.telecentric_projection_client = actionlib.SimpleActionClient( "project_telecentric", TelecentricProjectionAction) self.request_data_client = actionlib.SimpleActionClient( "request_data", RequestDataAction) for client in [ self.telecentric_projection_client, self.request_data_client ]: if not client.wait_for_server(rospy.Duration(TIMEOUT)): self.fail(msg="Request_data action servers timed out!") self.pc_subscriber = rospy.Subscriber("/projected_point_cloud", PointCloud2, self.callback) # Rotation, that is 90 degrees rotated to the original camera in the test self.trafo = Transform() self.trafo.rotation.x = 0 self.trafo.rotation.y = -0.7071068 self.trafo.rotation.z = 0 self.trafo.rotation.w = 0.7071068 self.trafo.translation.x = 0 self.trafo.translation.y = 0 self.trafo.translation.z = 0 # Test the projected depth image self.request_point_cloud() self.send_goal_depth_image() self.generate_line_estimate() # Test the point cloud self.send_goal_point_cloud() self.get_projected_point_cloud() # Test the subscribed cloud self.send_goal_with_publishing_point_cloud()
def run(self): """ The run function for this step Args: Yields: chuck_approach_pose (geometry_msgs/PoseStamped) : approach pose for impedance control with the chuck .. seealso:: :meth:`task_executor.abstract_step.AbstractStep.run` """ rospy.loginfo("Action {}: Detecting schunk.".format(self.name)) self._stopped = False # Ask for the schunk detector stub_tf = Transform() stub_pcl = PointCloud2() stub = TemplateMatchRequest() stub.initial_estimate = stub_tf stub.target_cloud = stub_pcl chuck_approach_pose = self._detect_schunk_srv(stub).template_pose self.notify_service_called( DetectSchunkAction.DETECT_SCHUNK_SERVICE_NAME) yield self.set_running() # Check the status of the server if self._stopped: yield self.set_preempted(action=self.name, srv=self._detect_schunk_srv.resolved_name, chuck_approach_pose=chuck_approach_pose) else: yield self.set_succeeded(chuck_approach_pose=chuck_approach_pose)
def calculate_transform(self, data): """ Takes a marker pose ROS message and returns a robot-base-frame-to-marker transform ROS message. Parameters: data - pose ROS message of marker relative to camera Returns: Transform ROS message of robot base frame to marker """ #calculate the transform in_x = data.position.x in_y = data.position.y in_z = data.position.z input_translation = [in_x, in_y, in_z] multiplier = np.array([[ -0.02025737, -0.31392, 0.04627322], [-0.38235706, 0.04113464, 0.03979437], [-0.03673691, -0.27182984, -0.36413172 ]], dtype=np.float) offset = np.array([0.45368236, -0.14424458, 0.8933589], dtype=np.float) output_translation = np.matmul(multiplier, input_translation)+ offset #build the transform output_transform = Transform() output_transform.translation.x = output_translation[0] output_transform.translation.y = output_translation[1] output_transform.translation.z = output_translation[2] #TODO: Check that the rotation transform is correct. output_transform.rotation = data.orientation return output_transform
def ros_publish_waypoint(self, action): """Publish single-point ROS trajectory message with given x, y and default z, att.""" # create trajectory msg traj = MultiDOFJointTrajectory() traj.header.stamp = rospy.Time.now() traj.header.frame_id = 'frame' traj.joint_names.append('base_link') # create end point for trajectory transforms = Transform() transforms.translation.x = action[0] transforms.translation.y = action[1] transforms.translation.z = Z quat = tf.transformations.quaternion_from_euler(0, 0, 0, axes='rzyx') transforms.rotation.x = quat[0] transforms.rotation.y = quat[1] transforms.rotation.z = quat[2] transforms.rotation.w = quat[3] velocities = Twist() accel = Twist() point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel], rospy.Time()) traj.points.append(point) self.waypoint_publisher.publish(traj)
def transformFromPose(cls, p): t = Transform() t.translation.x = p.position.x t.translation.y = p.position.y t.translation.z = p.position.z t.rotation = deepcopy(p.orientation) return t
def JointTrajectory2MultiDofTrajectory(self, joint_trajectory): multi_dof_trajectory = MultiDOFJointTrajectory() for i in range(0, len(joint_trajectory.points)): temp_point = MultiDOFJointTrajectoryPoint() temp_transform = Transform() temp_transform.translation.x = joint_trajectory.points[ i].positions[0] temp_transform.translation.y = joint_trajectory.points[ i].positions[1] temp_transform.translation.z = joint_trajectory.points[ i].positions[2] temp_transform.rotation.w = 1.0 temp_vel = Twist() temp_vel.linear.x = joint_trajectory.points[i].velocities[0] temp_vel.linear.y = joint_trajectory.points[i].velocities[1] temp_vel.linear.z = joint_trajectory.points[i].velocities[2] temp_acc = Twist() temp_acc.linear.x = joint_trajectory.points[i].accelerations[0] temp_acc.linear.y = joint_trajectory.points[i].accelerations[1] temp_acc.linear.z = joint_trajectory.points[i].accelerations[2] temp_point.transforms.append(temp_transform) temp_point.velocities.append(temp_vel) temp_point.accelerations.append(temp_acc) multi_dof_trajectory.points.append(temp_point) return multi_dof_trajectory
def affineToTransform(affine, summed_transform): transformation = Transform() if affine is not None: scalex = np.linalg.norm(affine[:, 0]) scalez = np.linalg.norm(affine[:, 1]) # offset camera center -> translate affected by scale t_offsetx = 320 * (1 - scalex) / 2 t_offsetz = 240 * (1 - scalez) / 2 # calc translation transformation.translation.y = 1 / ((scalex + scalez) / 2) transformation.translation.x = int( affine[0, 2] - t_offsetx) #* summed_transform.translation.y transformation.translation.z = int( affine[1, 2] - t_offsetz) #* summed_transform.translation.y # calc rotation affine[:, 0] /= scalex affine[:, 1] /= scalez yaw = math.atan2(affine[1, 0], affine[0, 0]) rotation = tf.transformations.quaternion_from_euler(0, 0, yaw) transformation.rotation.x = rotation[0] transformation.rotation.y = rotation[1] transformation.rotation.z = rotation[2] transformation.rotation.w = rotation[3] return transformation else: return None
def get_lines(self, fmt): t = Time() t.secs = 0 # MESSAGE_TO_TUM_SHORT line = ROSMsg2CSVLine.to(fmt, Point(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, PointStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POINTSTAMPED) line = ROSMsg2CSVLine.to(fmt, Vector3(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3) line = ROSMsg2CSVLine.to(fmt, Vector3Stamped(), t, ROSMessageTypes.GEOMETRY_MSGS_VECTOR3STAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovarianceStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCESTAMPED) line = ROSMsg2CSVLine.to( fmt, PoseWithCovariance(), t, ROSMessageTypes.GEOMETRY_MSGS_POSEWITHCOVARIANCE) line = ROSMsg2CSVLine.to(fmt, PoseStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_POSESTAMPED) line = ROSMsg2CSVLine.to(fmt, Pose(), t, ROSMessageTypes.GEOMETRY_MSGS_POSE) line = ROSMsg2CSVLine.to(fmt, Quaternion(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNION) line = ROSMsg2CSVLine.to( fmt, QuaternionStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_QUATERNIONSTAMPED) line = ROSMsg2CSVLine.to(fmt, Transform(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORM) line = ROSMsg2CSVLine.to( fmt, TransformStamped(), t, ROSMessageTypes.GEOMETRY_MSGS_TRANSFORMSTAMPED) return line
def simulate_mbes(self, mbes_ac): # Find particle's mbes pose without broadcasting/listening to tf transforms particle_tf = Transform() particle_tf.translation = self.pose.position particle_tf.rotation = self.pose.orientation mat_part = matrix_from_tf(particle_tf) trans_mat = np.dot(mat_part, self.mbes_tf_mat) trans = TransformStamped() trans.transform.translation.x = translation_from_matrix(trans_mat)[0] trans.transform.translation.y = translation_from_matrix(trans_mat)[1] trans.transform.translation.z = translation_from_matrix(trans_mat)[2] trans.transform.rotation = Quaternion( *quaternion_from_matrix(trans_mat)) # Build MbesSimGoal to send to action server mbes_goal = MbesSimGoal() mbes_goal.mbes_pose.header.frame_id = self.map_frame # mbes_goal.mbes_pose.child_frame_id = self.mbes_frame_id # The particles will be in a child frame to the map mbes_goal.mbes_pose.header.stamp = rospy.Time.now() mbes_goal.mbes_pose.transform = trans.transform # Get result from action server mbes_ac.send_goal(mbes_goal) mbes_ac.wait_for_result() mbes_res = mbes_ac.get_result() # Pack result into PointCloud2 mbes_pcloud = PointCloud2() mbes_pcloud = mbes_res.sim_mbes mbes_pcloud.header.frame_id = self.map_frame return mbes_pcloud
def from_dict(in_dict): """ Sets values parsed from a given dictionary. :param in_dict: input dictionary. :type in_dict: dict[string, string|dict[string,float]] :rtype: None """ global robot_base_frame global robot_effector_frame global tracking_base_frame global eye_on_hand global transformation eye_on_hand = in_dict['eye_on_hand'] robot_base_frame = in_dict['robot_base_frame'] tracking_base_frame = in_dict['tracking_base_frame'] transformation = TransformStamped( child_frame_id=in_dict['tracking_base_frame'], transform=Transform( Vector3(in_dict['transformation']['x'], in_dict['transformation']['y'], in_dict['transformation']['z']), Quaternion(in_dict['transformation']['qx'], in_dict['transformation']['qy'], in_dict['transformation']['qz'], in_dict['transformation']['qw']))) if eye_on_hand: robot_effector_frame = in_dict['robot_effector_frame'] transformation.header.frame_id = in_dict['robot_effector_frame'] else: transformation.header.frame_id = in_dict['robot_base_frame']