def graspit_grasp_pose_to_moveit_grasp_pose(move_group_commander, listener, graspit_grasp_msg, grasp_frame='/approach_tran'): """ :param move_group_commander: A move_group command from which to get the end effector link. :type move_group_commander: moveit_commander.MoveGroupCommander :param listener: A transformer for looking up the transformation :type tf.TransformListener :param graspit_grasp_msg: A graspit grasp message :type graspit_grasp_msg: graspit_msgs.msg.Grasp """ try: listener.waitForTransform(grasp_frame, move_group_commander.get_end_effector_link(), rospy.Time(0), timeout=rospy.Duration(1)) at_to_ee_tran, at_to_ee_rot = listener.lookupTransform( grasp_frame, move_group_commander.get_end_effector_link(), rospy.Time()) except: rospy.logerr( "graspit_grasp_pose_to_moveit_grasp_pose::\n " + "Failed to find transform from %s to %s" % (grasp_frame, move_group_commander.get_end_effector_link())) ipdb.set_trace() graspit_grasp_msg_final_grasp_tran_matrix = tf_conversions.toMatrix( tf_conversions.fromMsg(graspit_grasp_msg.final_grasp_pose)) approach_tran_to_end_effector_tran_matrix = tf.TransformerROS( ).fromTranslationRotation(at_to_ee_tran, at_to_ee_rot) if move_group_commander.get_end_effector_link() == 'l_wrist_roll_link': rospy.logerr('This is a PR2\'s left arm so we have to rotate things.') pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation( [0, 0, 0], tf.transformations.quaternion_from_euler(0, math.pi / 2, 0)) else: pr2_is_weird_mat = tf.TransformerROS().fromTranslationRotation( [0, 0, 0], [0, 0, 0]) actual_ee_pose_matrix = np.dot(graspit_grasp_msg_final_grasp_tran_matrix, approach_tran_to_end_effector_tran_matrix) actual_ee_pose_matrix = np.dot(actual_ee_pose_matrix, pr2_is_weird_mat) actual_ee_pose = tf_conversions.toMsg( tf_conversions.fromMatrix(actual_ee_pose_matrix)) rospy.loginfo("actual_ee_pose: " + str(actual_ee_pose)) return actual_ee_pose
def call_back(msg): # Global Frame: "/map" map_id = msg.header.frame_id br = tf.TransformBroadcaster() position = [ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ] quat = [msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w] r, p, y = tf.transformations.euler_from_quaternion(quat) tfros = tf.TransformerROS() M = tfros.fromTranslationRotation(position, quat) M_inv = np.linalg.inv(M) trans = tf.transformations.translation_from_matrix(M_inv) rot = tf.transformations.quaternion_from_matrix(M_inv) t = rospy.Time.from_sec(0) #t = rospy.Time.now() br.sendTransform(trans, rot, t, map_id, "/X1/base_footprint") #quat = tf.transformations.quaternion_from_euler (-r, -p, -y) br.sendTransform((0, 0, 0), \ (0, 0, 0, 1), rospy.Time.now(), "map", "X1/odom")
def __init__(self): #/vel_based_pos_traj_controller/ self.client = actionlib.SimpleActionClient('icl_phri_ur5/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print ("Waiting for server...") self.client.wait_for_server() print ("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) print ("Init done") self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() joint_weights = [12,5,4,3,2,1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) self.sub_cancel = rospy.Subscriber('/voice_command', Int32, self.cancel_cb) self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller') self.gripper_ac.wait_for_server() self.gripper_ac.initiate() self.gripper_ac.send_goal(0.10) self.gripper_ac.wait_for_result() self.cancel = False
def setupNode(self): rospy.init_node("i3dr_set_tf_parent", anonymous=True, disable_signals=True) self.map_to_robot_base_tf = None self.tf_ready = False self.frame_4_id = rospy.get_param( '~frame_4_id', "base_link") #base_link / ur10_base_link self.frame_3_id = rospy.get_param('~frame_3_id', "tool0") #tool0 / end_effector self.frame_1_id = rospy.get_param('~frame_1_id', "map") self.frame_2_id = rospy.get_param('~frame_2_id', "i3dr_stereo_base_link") self.output_frame_1_id = rospy.get_param('~output_frame_1_id', "map") self.output_frame_2_id = rospy.get_param('~output_frame_2_id', "ur10_base_link") self.update_mode = rospy.get_param('~update_mode', "continuous") self.service_name = rospy.get_param('~service_name', "i3dr_get_scan_home") if self.update_mode == "continuous": self.service_update = False elif self.udpate_mode == "service": self.service_update = True if self.service_update: rospy.Service(self.service_name, Empty, self.handle_get_tfs) self.tfListener = tf.TransformListener() self.tfTransformer = tf.TransformerROS() self.tfBroadcaster = tf.TransformBroadcaster()
def __init__( self ): # This function is called one time as soon as an instance of a class is created #self.rate = 100 #[Hz] # Declaring a subscriber. Parameters: topic name (string), topic data type, callback function # Every time another node publishes a message to "/arbitrary_subscribed_topic_name", we will call the function called self.arbitrary_topic_name_cb # The "/" before the topic name is important #rospy.Subscriber("/usb_cam/image_raw", Image, self.CameraCalibration_cb) This was me testing #rospy.Subscriber("/toppings", DetectionArray, self.CameraCalibration_cb) #rospy.Subscriber("/self", DetectionArray, self.CameraCalibration_cb) # Declaring a publisher. Parameters: topic name (string), topic data type, queue_size (use queue_size=10 as default) #self.CameraCalibration_pub = rospy.Publisher("/worldxyz", Float32MultiArray, queue_size=10) # Do any other initialization of class #scaling factors for millimeters to pixel conversion #this is related to the self.tf_listener = tf.TransformListener() self.tf_transformer = tf.TransformerROS(True, rospy.Duration(10.0)) self.sx = 1 / 642.5793848798857 #fx self.sy = 1 / 644.8809875234275 #fy self.cx = 320 #center of image in pixels self.cy = 240 self.pixelUV = [320, 240 ] #initialize pixel coords and orientation for testing self.pixelOrientation = radians(45) self.CameraAngle = radians( 10) #angle of camera lens relative to vertical self.CameraHeight = 839 self.z0 = 839 / cos( self.CameraAngle ) #851.942927372 absolute distance of camera lens from table in millimeters self.CameraLenstoOrigin = [271, 36.6, 52] #in world frame #self.CameraLenstoOrigin = [-80 , 36.6 , 52] #Rotation is 180 minus camera angle about x self.rotationMatrixX = np.array( [[1, 0, 0], [0, cos(pi - self.CameraAngle), -sin(pi - self.CameraAngle)], [0, sin(pi - self.CameraAngle), cos(pi - self.CameraAngle)]] ) #rotation from world frame to camera frame -should just be a rotation about world x-axis #Rotate 90 about z self.rotationMatrixZ = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) self.rotationMatrix = np.matmul(self.rotationMatrixZ, self.rotationMatrixX) # self.rotationMatrix = np.array([[1,0,0], # [0,1,0], # [0,0,1]]) self.translationVector = np.array([ self.CameraLenstoOrigin[0], self.CameraLenstoOrigin[1], self.CameraLenstoOrigin[2] ]) #translation from world frame to camera frame
def __init__(self): self.tf = tf.TransformerROS(True, rospy.Duration(10.0)) # self.listener = tf.TransformListener() t1 = TransformStamped() # /alvar /nut_start t1.header.frame_id = '/ar_marker_17' t1.child_frame_id = '/nutStart' # t1.transform.translation = Vector3(-0.057, 0.050, -0.012) # t1.transform.translation = Vector3(-0.052, 0.164, -0.009) t1.transform.translation = Vector3(-0.052, 0.194, -0.009) qua_t1 = self.r_to_q([0, 0, -math.pi / 2], [0, 0, 0, 1]) t1.transform.rotation = Quaternion(qua_t1[0], qua_t1[1], qua_t1[2], qua_t1[3]) ''' t2 = TransformStamped() # /camera /alvar (trans, rot) = self.listener.lookupTransform('/movo_camera_color_optical_frame', '/ar_marker_17', rospy.Time(0)) t2.header.frame_id = '/movo_camera_color_optical_frame' t2.child_frame_id = '/ar_marker_17' t2.transform.translation = Vector3(trans[0], trans[1], trans[2]) t2.transform.rotation = Quaternion(rot[0], rot[1], rot[2], rot[3]) ''' t3 = TransformStamped() # /left_ee /finger t3.header.frame_id = '/left_ee_link' t3.child_frame_id = '/finger' t3.transform.translation.x = 0.043 t3.transform.translation.z = 0.02 t3.transform.rotation = Quaternion(0, 0, 0, 1) self.tf.setTransform(t1) # self.tfROS.setTransform(t2) self.tf.setTransform(t3)
def __init__(self, time_, grippers): self.inputArgs = ['relativeTemp', 'absoluteTemp', 'self.mode'] self.verificationArgs = ['self.taskDone'] self.pointTime = time_ # height from world frame (i.e. table) and not yumi_base_link # Otherise the frame is yumi_base_link, [Right, Left], for combined only right is used self.transformer = tf.TransformerROS(True, rospy.Duration(1.0)) self.gripper = grippers # in mm, [Right, left]
def __init__(self): # Variables self.node_name = rospy.get_name() self.img = None self.cv_bridge = CvBridge() self.totem_list = [] # (x, y, z, color) self.color_detect = ColorDetectHSV() self.wamv_position = None self.tf_listener = tf.TransformListener() self.tf_transformer = tf.TransformerROS() self.translation_lidar_camera = [0.0, 0.13, 0.5] self.quaternion_lidar_camera = [0, 0, -0.7068851, 0.7073284] self.camera_matrix = [ 1400.69, 0.0, 971.463, 0.0, 1400.69, 535.668, 0.0, 0.0, 1.0 ] #self.translation_lidar_camera = [-0.350, 0.000, 0.291] #self.quaternion_lidar_camera = [ 0, 0, -0.707, 0.707 ] #self.camera_matrix = [762.7249337622711, 0.0, 640.5, 0.0, 762.7249337622711, 360.5, 0.0, 0.0, 1.0] self.totem_circle_command = [] self.start = False rospy.Timer(rospy.Duration(0.5), self.circle_path_planning) #self.add_wp = rospy.ServiceProxy("/add_waypoint", waypoint) #self.start_wp = rospy.ServiceProxy("/start_waypoint_nav", Trigger) # Ros service self.srv_totem_cirle = rospy.Service('~set_totem_cirle', SetTotemCircle, self.cb_srv_totem_circle) self.srv_totem_cirle = rospy.Service('~clear', Trigger, self.cb_srv_clear) self.srv_totem_cirle = rospy.Service('~start', Trigger, self.cb_srv_start) # Publisher self.pub_image_roi = rospy.Publisher("~image_roi", Image, queue_size=1) self.pub_image_roi_compressed = rospy.Publisher( "~image_roi/compressed", CompressedImage, queue_size=1) self.pub_image_mask = rospy.Publisher("~image_mask", Image, queue_size=1) self.pub_marker = rospy.Publisher("~totem_marker", MarkerArray, queue_size=1) # Subscriber self.sub_image = rospy.Subscriber("~image_raw_compressed", CompressedImage, self.cb_image, queue_size=1) self.sub_obj_list = rospy.Subscriber("~obj_list", ObjectPoseList, self.cb_obj_list, queue_size=1) self.sub_odometry = rospy.Subscriber("/odometry/filtered", Odometry, self.cb_odometry, queue_size=1)
def getState(self, posData): # odomEstimate = self.getOdomEstimate() # velxEstimate = odomEstimate.twist.twist.linear.x # velyEstimate = odomEstimate.twist.twist.linear.y pose = posData.pose[1] position = pose.position orientation = pose.orientation t = tf.TransformerROS(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'world' m.child_frame_id = 'base_link' m.transform.translation.x = position.x m.transform.translation.y = position.y m.transform.translation.z = position.z m.transform.rotation.x = orientation.x m.transform.rotation.y = orientation.y m.transform.rotation.z = orientation.z m.transform.rotation.w = orientation.w (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([orientation.x, orientation.y, orientation.z, orientation.w]) t.setTransform(m) velx = posData.twist[1].linear.x vely = posData.twist[1].linear.y velz = posData.twist[1].linear.z # Transform to body frame velocities velVector = Vector3Stamped() velVector.vector.x = velx velVector.vector.y = vely velVector.vector.z = velz velVector.header.frame_id = "world" # velVectorTransformed = self.tl.transformVector3("base_link", velVector) velVectorTransformed = t.transformVector3("base_link", velVector) velxBody = velVectorTransformed.vector.x velyBody = velVectorTransformed.vector.y velzBody = velVectorTransformed.vector.z carTangentialSpeed = math.sqrt(velx ** 2 + vely ** 2) carAngularVel = posData.twist[1].angular.z stateInfo = { "x": position.x, "y": position.y, "i": orientation.x, "j": orientation.y, "k": orientation.z, "w": orientation.w, "xdot": velx, "ydot": vely, "thetadot": carAngularVel, "s": carTangentialSpeed, "xdotbodyframe": velxBody, "ydotbodyframe": velyBody } state = [] for key in self.state_info: state.append(stateInfo[key]) #rewardState = [stateInfo["thetadot"], stateInfo["xdotbodyframe"], stateInfo["ydotbodyframe"]] return np.array(state)
def __init__(self): print 'creating Turtlebot' self.transform = tf.TransformerROS() self.pose = Pose() pos = Point() quarter = Quaternion() self.pose.position.x = 1.23 self.pose.position.y = 0.92 self.pose.position.z = 0 self.path = [] self.initPos = [0, 0, 0] self.initOrient = [0, 0, 0, 0] self.hopeful = [0, 0, 0] self.isInit = False self.pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, None, queue_size=10) self.goal_sub = rospy.Subscriber('goal_pose', PoseStamped, self.executeAStar, queue_size=1) #self.bump_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.readBumper, queue_size=1) #self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odometryCallback) self.odom_tfList = tf.TransformListener() self.odom_broad = tf.TransformBroadcaster() rospy.Timer(rospy.Duration(.01), self.odometryCallback) print 'Completed making Turtlebot'
def call_back(self, msg): obj_list = msg cluster_num = obj_list.size for i in range(cluster_num): tf_points = PoseArray() tf_points = obj_list.list[i].pcl_points centroids = Point() centroids = obj_list.list[i].position self.image = np.zeros((int(self.height), int(self.width), 3), np.uint8) plane_xy = [] plane_yz = [] plane_xz = [] pcl_size = len(tf_points.poses) # ======= Coordinate transform for better project performance ====== position = [0, 0, 0] rad = math.atan2(centroids.y, centroids.x) quaternion = tf.transformations.quaternion_from_euler(0., 0., -rad) transformer = tf.TransformerROS() transpose_matrix = transformer.fromTranslationRotation( position, quaternion) for m in range(pcl_size): new_x = tf_points.poses[m].position.x new_y = tf_points.poses[m].position.y new_z = tf_points.poses[m].position.z orig_point = np.array([new_x, new_y, new_z, 1]) new_center = np.dot(transpose_matrix, orig_point) tf_points.poses[m].position.x = new_center[0] tf_points.poses[m].position.y = new_center[1] tf_points.poses[m].position.z = new_center[2] # ======= project to XY, YZ, XZ plane ======= for j in range(pcl_size): plane_xy.append([ tf_points.poses[j].position.x, tf_points.poses[j].position.y ]) plane_yz.append([ tf_points.poses[j].position.y, tf_points.poses[j].position.z ]) plane_xz.append([ tf_points.poses[j].position.x, tf_points.poses[j].position.z ]) self.toIMG(pcl_size, plane_xy, 'xy') self.toIMG(pcl_size, plane_yz, 'yz') self.toIMG(pcl_size, plane_xz, 'xz') model_type = self.classify() # *************************************************************** # Add to object list # *************************************************************** obj_list.list[i].type = model_type #cv2.imwrite( "Image" + str(self.index) + ".jpg", self.image) #self.index = self.index + 1 #print "Save image" self.pub_obj.publish(obj_list) self.drawRviz(obj_list)
def cbOdom(self, msg): # Global Frame: "/map" # map_id = msg.header.frame_id # br = tf.TransformBroadcaster() position = [ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ] quat = [msg.pose.pose.orientation.x,\ msg.pose.pose.orientation.y,\ msg.pose.pose.orientation.z,\ msg.pose.pose.orientation.w] r, p, y = tf.transformations.euler_from_quaternion(quat) tfros = tf.TransformerROS() M = tfros.fromTranslationRotation(position, quat) # M_inv = np.linalg.inv(M) self.odom_trans = tf.transformations.translation_from_matrix(M) self.odom_rot = tf.transformations.quaternion_from_matrix(M) # t = rospy.Time.from_sec(0) # t = rospy.Time.now() # br.sendTransform(trans, rot, t, "/robot_base", map_id) #quat = tf.transformations.quaternion_from_euler (-r, -p, -y) # br.sendTransform((0, 0, 0), \ # (0, 0, 0, 1), rospy.Time.now(), "/laser", "/robot_base") self.get_odom = True
def __init__(self): #/vel_based_pos_traj_controller/ self.client = actionlib.SimpleActionClient( 'icl_phri_ur5/follow_joint_trajectory', FollowJointTrajectoryAction) self.goal = FollowJointTrajectoryGoal() self.goal.trajectory = JointTrajectory() self.goal.trajectory.joint_names = JOINT_NAMES print("Waiting for server...") self.client.wait_for_server() print("Connected to server") joint_states = rospy.wait_for_message("joint_states", JointState) print(joint_states) joint_states = list(deepcopy(joint_states).position) del joint_states[-1] self.joints_pos_start = np.array(joint_states) print("Init done") self.listener = tf.TransformListener() self.Transformer = tf.TransformerROS() joint_weights = [12, 5, 4, 3, 2, 1] self.ik = InverseKinematicsUR5() self.ik.setJointWeights(joint_weights) self.ik.setJointLimits(-pi, pi) # self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) # pickplace_sub = message_filters.Subscriber('pick', Int32) # tag_sub = message_filters.Subscriber('tag', Int32) # ts = message_filters.ApproximateTimeSynchronizer([pickplace_sub, tag_sub], 10, 0.1, allow_headerless=True) # ts.registerCallback(self.pickplace_cb) self.sub_pickplace = rospy.Subscriber('/CHOICE', String, self.pickplace_cb)
def __init__(self): self.rate = 100 #[Hz] self.CameraAngle = None#math.radians(self.CAMERA_TILT) #angle of camera lens relative to vertical self.z0 = 0.0 self.CameraHeight = 839 rospy.Subscriber("/usb_cam/image_raw", Image, self.image_cb) # Need to use rectified image instead self.toppings_pub = rospy.Publisher("/toppings", DetectionArray, queue_size=10) self.slots_pub = rospy.Publisher("/slots", DetectionArray, queue_size=10) rospy.Subscriber("/camera_angle", Float32, self.camera_angle_cb, queue_size = 1) self.bridge = CvBridge() self.image_dims = [] # Camera_Calibration Parameters self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() rospy.sleep(0.2) self.tf_transformer = tf.TransformerROS(True, rospy.Duration(10.0)) self.sx = 1/642.5793848798857 #fx self.sy = 1/644.8809875234275 #fy self.cx = 185 #center of image in pixels self.cy = 230 '''self.pixelUV = [320,240] #initialize pixel coords and orientation for testing
def test_transformer_ros(self): tr = tf.TransformerROS() m = geometry_msgs.msg.TransformStamped() m.header.stamp = rospy.Time().from_sec(3.0) m.header.frame_id = "PARENT" m.child_frame_id = "THISFRAME" m.transform.translation.y = 5.0 m.transform.rotation.x = 0.04997917 m.transform.rotation.y = 0 m.transform.rotation.z = 0 m.transform.rotation.w = 0.99875026 tr.setTransform(m) m.header.stamp = rospy.Time().from_sec(5.0) tr.setTransform(m) # Smoke the various transform* methods types = [ "Point", "Pose", "Quaternion", "Vector3" ] for t in types: msg = getattr(geometry_msgs.msg, "%sStamped" % t)() msg.header.frame_id = "THISFRAME" msg_t = getattr(tr, "transform%s" % t)("PARENT", msg) self.assertEqual(msg_t.header.frame_id, "PARENT") # PointCloud is a bit different, so smoke is different msg = sensor_msgs.msg.PointCloud() msg.header.frame_id = "THISFRAME" msg.points = [geometry_msgs.msg.Point32(1,2,3)] xmsg = tr.transformPointCloud("PARENT", msg) self.assertEqual(xmsg.header.frame_id, "PARENT") self.assertEqual(len(msg.points), len(xmsg.points)) self.assertNotEqual(msg.points[0], xmsg.points[0]) """ Two fixed quaternions, a small twist around X concatenated. >>> t.quaternion_from_euler(0.1, 0, 0) array([ 0.04997917, 0. , 0. , 0.99875026]) >>> t.quaternion_from_euler(0.2, 0, 0) array([ 0.09983342, 0. , 0. , 0.99500417]) """ # Specific test for quaternion types msg = geometry_msgs.msg.QuaternionStamped() q = [ 0.04997917, 0. , 0. , 0.99875026 ] msg.quaternion.x = q[0] msg.quaternion.y = q[1] msg.quaternion.z = q[2] msg.quaternion.w = q[3] msg.header.stamp = rospy.Time().from_sec(3.0) msg.header.frame_id = "THISFRAME" msg_t = tr.transformQuaternion("PARENT", msg) self.assertEqual(msg_t.header.frame_id, "PARENT") for a,v in zip("xyzw", [ 0.09983342, 0. , 0. , 0.99500417]): self.assertAlmostEqual(v, getattr(msg_t.quaternion, a), 4)
def __init__(self): self.pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, None, queue_size=10) #self.bump_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.readBumper, queue_size=1) self.goal_sub = rospy.Subscriber('move_base_simple/goal', PoseStamped, self.navToPose, queue_size=1) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.readOdom) self.odom_lis = tf.TransformListener() self.odom_bro = tf.TransformBroadcaster() self.odom_bro.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "base_footprint", "odom") rospy.sleep self.trans = tf.TransformerROS() self.posn = [0, 0, 0] self.desired = [0, 0, 0] self.kp_lin = 1 self.kp_ang = 1 self.map = Map()
def __init__(self, bag): """ Create a new BagTfTransformer from an open rosbag or from a file path :param bag: an open rosbag or a file path to a rosbag file """ if type(bag) == str: bag = rosbag.Bag(bag) self.tf_messages = sorted((self._remove_slash_from_frames(tm) for m in bag if m.topic.strip("/") == 'tf' for tm in m.message.transforms), key=lambda tfm: tfm.header.stamp.to_nsec()) self.tf_static_messages = sorted( (self._remove_slash_from_frames(tm) for m in bag if m.topic.strip("/") == 'tf_static' for tm in m.message.transforms), key=lambda tfm: tfm.header.stamp.to_nsec()) self.tf_times = np.array( list((tfm.header.stamp.to_nsec() for tfm in self.tf_messages))) self.transformer = tf.TransformerROS() self.last_population_range = (rospy.Time(0), rospy.Time(0)) self.all_frames = None self.all_transform_tuples = None self.static_transform_tuples = None
def check_transform(self, event): t_base_marker = self.getTransform("/arips_base", "/ar_marker_0") #print("t_base_marker", t_base_marker t_arm_tool0 = self.getTransform("/base_link", "/tool0") t_tool0_marker = tf.TransformerROS().fromTranslationRotation([-0.0475, 0.001, 0.03], [0, 0, 0, 1]) t_arm_marker = np.matmul(t_arm_tool0, t_tool0_marker) t_marker_arm = np.linalg.inv(t_arm_marker) t_base_arm_corrected = np.matmul(t_base_marker, t_marker_arm); np.set_printoptions(formatter={'float': lambda x: "{0:0.4f}".format(x)}) #print("t_arm_base_corrected" #print(t_arm_base_corrected trans = tf.transformations.translation_from_matrix(t_base_arm_corrected) quat = tf.transformations.quaternion_from_matrix(t_base_arm_corrected) print("t_base_arm_corrected") print(t_base_arm_corrected) print("transform") print(trans, quat) print()
def getClosestKey(self, req): query = req.query keys = self.r.keys(query) keys = list(filter(lambda x: '/pose' in x and 'target' not in x, keys)) min_distance = float('inf') min_key = None for key in keys: pose = PoseStamped() pose.header.frame_id = self.fixed_frame pose.header.stamp = rospy.Time.now() pose.pose = self.readPose(key) t = tf.TransformerROS() p = t.transformPose('/map', pose) distance = sqrt(p.pose.position.x**2 + p.pose.position.y**2 + p.pose.position.z**2) if distance < min_distance: min_distance = distance min_key = key min_key = min_key.replace('/pose', '') rospy.loginfo("Key: " + min_key) return GetKeyResponse(min_key)
def __init__(self): print "making robot" self.trans = tf.TransformerROS() self.pose = Pose() posn = Point() quat = Quaternion() self.pose.position = posn self.pose.orientation = quat self.pose.position.x = 0 self.pose.position.y = 0 self.pose.position.z = 0 self.initpos = [0,0,0] self.initort = [0,0,0,0] self.desired = [0,0,0] self.kp_lin = .5 self.kp_ang = .5 self.gotInit = False self.pub = rospy.Publisher('cmd_vel_mux/input/teleop', Twist, None, queue_size=10) #self.bump_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.readBumper, queue_size=1) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.readOdom) self.odom_lis = tf.TransformListener() self.odom_bro = tf.TransformBroadcaster() self.doWavefront() self.goal_sub = rospy.Subscriber('move_base_simple/goal', PoseStamped, self.doAstar, queue_size=1) print "made robot"
def __init__(self): rospy.init_node('spacenav_remap') self.linear_gain = 500 self.angular_gain = 500 self.behavior = rospy.get_param('~behavior', 'wrench') behaviors = ['wrench', 'velocity', 'position'] assert self.behavior in behaviors, "Parameter 'behavior' invalid, must be in {}".format( behaviors) if self.behavior == 'wrench': self.wrench_pub = rospy.Publisher('/wrench', WrenchStamped, queue_size=1) else: raise (NotImplementedError( "Velocity and position control by mouse are not yet supported") ) self.transformer = tf.TransformerROS() self.world_to_body = np.eye(3) self.odom_sub = rospy.Subscriber('/truth/odom', nav_msgs.Odometry, self.odom_cb, queue_size=1) self.twist_sub = rospy.Subscriber('/spacenav/twist', Twist, self.twist_cb, queue_size=1)
def convert_cloud_camera_to_map(self, cloud, transform): target_camera_frame = "/map" starting_camera_frame = cloud.header.frame_id transformation_store = tf.TransformerROS() transformation_store.setTransform(transform) t = transformation_store.getLatestCommonTime(target_camera_frame, starting_camera_frame) tr_r = transformation_store.lookupTransform(target_camera_frame, starting_camera_frame, t) tr = Transform() tr.translation = Vector3(tr_r[0][0],tr_r[0][1],tr_r[0][2]) tr.rotation = Quaternion(tr_r[1][0],tr_r[1][1],tr_r[1][2],tr_r[1][3]) tr_s = TransformStamped() tr_s.header = std_msgs.msg.Header() tr_s.header.stamp = rospy.Time.now() tr_s.header.frame_id = target_camera_frame tr_s.child_frame_id = starting_camera_frame tr_s.transform = tr t_kdl = self.transform_to_kdl(tr_s) points_out = [] for p_in in pc2.read_points(cloud,field_names=["x","y","z","rgb"]): p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2]) points_out.append([p_out[0],p_out[1],p_out[2],p_in[3]]) res = pc2.create_cloud(cloud.header, cloud.fields, points_out) rospy.loginfo("done") return res
def __init__(self): init_node("qr_code_manager", anonymous=True) self.markers = dict() self.init_markers() self.pub = Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=5) self.stamp_string_pub = Publisher( "/visp_auto_tracker/code_message_stamped", StringStamped, queue_size=5) self.stamp_string_sub = Subscriber("/visp_auto_tracker/code_message", String, self.stamp_string_callback) self.stamp_position_sub = Subscriber( "/visp_auto_tracker/object_position_covariance", PoseWithCovarianceStamped, self.stamp_position_callback) self.code_stamped = StringStamped() self.position_stamped = PoseWithCovarianceStamped() self.baseLinkFrameId = rospy.get_param('~base_link_frame_id', '/base_link') self.mapFrameId = rospy.get_param('~map_frame_id', '/map') self.tr = tf.TransformerROS() self.tl = tf.TransformListener() self.d = {}
def __init__(self): rospy.Subscriber('/tf', tf2_msgs.msg.TFMessage, self.frame_handler) self.tf_baxter = tf.TransformerROS() self.trans_left = None self.trans_right = None
def __init__(self, position, orientation, fixtureHeight, fixtureRadius): self.position = position self.orientation = orientation self.fixtureHeight = fixtureHeight self.fixtureRadius = fixtureRadius transformer = tf.TransformerROS(True, rospy.Duration(0.1)) tfMatrix1 = transformer.fromTranslationRotation(translation=position, rotation=orientation) self.position2 = tfMatrix1.dot(np.array([0.025, 0, 0, 1]))[0:3]
def __init__(self, side): self.side = side self.last_timestamp_used = 0. self.jMt_calib_transformations = pickle.load( open('tMj_' + self.side + '_hand.pickle', 'r')) self.tf_pub = tf2_ros.TransformBroadcaster() self.tf_transformer = tf.TransformerROS() rospy.Subscriber('/agimus/vision/tags', TransformStamped, self.republishJointPose)
def __init__(self): ''' - Allow the user to move a "ghost" of the sub in Rviz, and then command a pose and orientation When determining left and right, the wire faces away from you''' rospy.init_node('spacenav_positioning') if sys.argv[1] == '2d' or sys.argv[1] == '3d': self.mode = sys.argv[1] else: rospy.loginfo('Invalid mode - Defaulting to 2D') self.mode = '2d' self.distance_marker = Marker() self.distance_marker.type = self.distance_marker.TEXT_VIEW_FACING self.distance_marker.color.r = 1 self.distance_marker.color.b = 1 self.distance_marker.color.g = 1 self.distance_marker.color.a = 1 self.distance_marker.scale.z = 0.1 self.transformer = tf.TransformerROS() self.world_to_body = np.eye(3) self.cur_position = None self.cur_orientation = None self.diff_position = 0 self.target_depth = 0 self.target_distance = 0 self.target_position = np.zeros(3) self.target_orientation = np.eye(3) self.target_orientation_quaternion = np.array([0.0, 0.0, 0.0, 1.0]) self.client = actionlib.SimpleActionClient('/moveto', mil_msgs.MoveToAction) # self.client.wait_for_server() self.target_pose_pub = rospy.Publisher('/posegoal', PoseStamped, queue_size=1) self.target_distance_pub = rospy.Publisher('/pose_distance', Marker, queue_size=1) self.odom_sub = rospy.Subscriber('/odom', nav_msgs.Odometry, self.odom_cb, queue_size=1) self.twist_sub = rospy.Subscriber('/spacenav/twist', Twist, self.twist_cb, queue_size=1) self.joy_sub = rospy.Subscriber('/spacenav/joy', Joy, self.joy_cb, queue_size=1)
def listener(fname, recreate_tf_static=True): """ Prepare the subscribers and setup the plot etc :param fname: The name of the file to start :param recreate_tf_static: True if the tf_static message saved in data/tf_static_dump.pkl should be loaded. This needs to be set to True if the bag that will be played does not contain the single tf_static msg that is usually right at the beginning of the bag file. """ global visuals, transformer rospy.init_node('listener_laser_scan', anonymous=True) transformer = tf.TransformerROS(True) # print("FrameList:\t" + transformer.allFramesAsString()) # Start the subscriber(s) rospy.Subscriber("tracked_objects/scan", TrackedLaserScan, callback_tracking) # General subscriber (tracking data) # Currently using _static here because plotting should happen upon receiving lidar data (incl c2x plotting) rospy.Subscriber("tf", TFMessage, callback_tf_static) # Acquire transform messages rospy.Subscriber("/FASCarE_ROS_Interface/car2x_objects", TrackedOrientedBoxArray, callback_c2x_tf) rospy.Subscriber("tf_static", TFMessage, callback_tf_static ) # Acquire static transform message for "ibeo" frames # rospy.Subscriber("tracked_objects/scan", TrackedLaserScan, callback_org_data) # now start a rosbag play for that filename FNULL = open(os.devnull, 'w') # redirect rosbag play output to devnull to suppress it play_rate = 0.15 # set the number to whatever you want your play-rate to be # play_rate = 1 rate = '-r' + str(play_rate) # using '-r 1' is the usual playback speed - this works, but since the code lags behind (cant process everything # in realtime), you will then get results after the bag finished playing (cached results) # using '-r 0.25' is still too fast for maven-1.bag # using '-r 0.2' works (bag finishes and no more associations are made on buffered data afterwards) start_time = 0 # time at which the bag should start playing time = '-s ' + str(start_time) if recreate_tf_static: pkl_filename = "./src/T2TF_SST/data/" # folder pkl_filename += "tf_static_dump.pkl" # filename with open(pkl_filename, 'rb') as pklinput: tf_static_data = pickle.load(pklinput) callback_tf_static(tf_static_data) player_proc = subprocess.Popen(['rosbag', 'play', rate, time, fname], cwd="./", stdout=FNULL) plt.show() # DON'T NEED TO SPIN IF YOU HAVE A BLOCKING plt.show # Kill the process (if it was started) player_proc.terminate()
def __init__(self): self.tf_listener = tf.TransformListener() self.Transformer = tf.TransformerROS() self.ee_link = '/ee_link' self.base_link = '/base_link' self.ik = InverseKinematicsUR5() self.ik.setEERotationOffsetROS() self.ik.setJointWeights([6, 5, 4, 3, 2, 1]) self.ik.setJointLimits(-pi, pi)
def pub_tf(self, position, quaternion): r, p, y = tf.transformations.euler_from_quaternion(quaternion) tfros = tf.TransformerROS() M = tfros.fromTranslationRotation(position, quaternion) M_inv = np.linalg.inv(M) trans = tf.transformations.translation_from_matrix(M_inv) rot = tf.transformations.quaternion_from_matrix(M_inv) self.br.sendTransform(trans, rot, rospy.Time.now(), "map", "/X1/base_footprint")