class RiCDiffCloseLoop(Device): def __init__(self, param, output): Device.__init__(self, param.getCloseDiffName(), output) self._baseLink = param.getCloseDiffBaseLink() self._odom = param.getCloseDiffOdom() self._maxAng = param.getCloseDiffMaxAng() self._maxLin = param.getCloseDiffMaxLin() self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz()) self._broadCase = TransformBroadcaster() Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1) Service('%s/setOdometry' % self._name, set_odom, self.setOdom) def diffServiceCallback(self, msg): if msg.angular.z > self._maxAng: msg.angular.z = self._maxAng elif msg.angular.z < -self._maxAng: msg.angular.z = -self._maxAng if msg.linear.x > self._maxLin: msg.linear.x = self._maxLin elif msg.linear.x < -self._maxLin: msg.linear.x = -self._maxLin # print msg.angular.z, msg.linear.x self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend()) def setOdom(self, req): self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend()) return set_odomResponse(True) def publish(self, data): q = Quaternion() q.x = 0 q.y = 0 q.z = data[6] q.w = data[7] odomMsg = Odometry() odomMsg.header.frame_id = self._odom odomMsg.header.stamp = rospy.get_rostime() odomMsg.child_frame_id = self._baseLink odomMsg.pose.pose.position.x = data[0] odomMsg.pose.pose.position.y = data[1] odomMsg.pose.pose.position.z = 0 odomMsg.pose.pose.orientation = q self._pub.publish(odomMsg) traMsg = TransformStamped() traMsg.header.frame_id = self._odom traMsg.header.stamp = rospy.get_rostime() traMsg.child_frame_id = self._baseLink traMsg.transform.translation.x = data[0] traMsg.transform.translation.y = data[1] traMsg.transform.translation.z = 0 traMsg.transform.rotation = q self._broadCase.sendTransformMessage(traMsg)
class FiducialTFPublisher(object): def __init__(self, fiducial_topic, fiducial_id, frame_id, invert=False): self._fiducial_id = fiducial_id self._frame_id = frame_id self._invert = invert self._fid_sub = rospy.Subscriber(fiducial_topic, FiducialTransformArray, callback=self._callback, queue_size=10) self._tf_brd = TransformBroadcaster() def _callback(self, fiducial_transforms): header = fiducial_transforms.header child_frame_id = self._frame_id if self._invert: child_frame_id = header.frame_id header.frame_id = self._frame_id obj = filter(lambda a: a.fiducial_id == self._fiducial_id, fiducial_transforms.transforms) if len(obj) > 0: obj = obj[0] if self._invert: trans = obj.transform.translation rot = obj.transform.rotation trans = [trans.x, trans.y, trans.z] rot = [rot.x, rot.y, rot.z, rot.w] m_trans = t.translation_matrix(trans) m_rot = t.quaternion_matrix(rot) transform = t.concatenate_matrices(m_trans, m_rot) inverse_transform = t.inverse_matrix(transform) trans = t.translation_from_matrix(inverse_transform) rot = t.quaternion_from_matrix(inverse_transform) obj.transform.translation = Vector3(x=trans[0], y=trans[1], z=trans[2]) obj.transform.rotation = Quaternion(x=rot[0], y=rot[1], z=rot[2], w=rot[3]) self._tf_brd.sendTransformMessage( TransformStamped(header=header, child_frame_id=child_frame_id, transform=obj.transform))
def set_map_pose(pose, parent_frame_id, child_frame_id): br = TransformBroadcaster() m = TransformStamped() m.header.frame_id = parent_frame_id m.header.stamp = rospy.Time.now() m.child_frame_id = child_frame_id m.transform.translation.x = pose.position.x m.transform.translation.y = pose.position.y m.transform.translation.z = pose.position.z m.transform.rotation.x = pose.orientation.x m.transform.rotation.y = pose.orientation.y m.transform.rotation.z = pose.orientation.z m.transform.rotation.w = pose.orientation.w br.sendTransformMessage(m)
class pathViewer: def __init__(self, pubName="path"): self.topicName = pubName self.interFrameMotions = [] self.br = TransformBroadcaster() def loadFromFile(self, Dir): fileNames = os.listdir(Dir) ###add original Transform origin = TransformStamped() origin.header.frame_id = "world" origin.child_frame_id = self.topicName origin.transform.rotation.w = 1 self.interFrameMotions.append(origin) count = 0 for f in fileNames: with open(Dir + "/" + f, "r") as current: Rtheta, C = pickle.load(current) q = quaternion_from_euler(radians(Rtheta[0]), radians(Rtheta[1]), radians(Rtheta[2]), 'szxy') count += 1 print(f) latestPose = TransformStamped() latestPose.header.frame_id = self.interFrameMotions[ -1].child_frame_id latestPose.child_frame_id = self.topicName + "/" + f[:f.rfind( ".")] latestPose.transform.translation.x = C[0, 0] latestPose.transform.translation.y = C[1, 0] latestPose.transform.translation.z = C[2, 0] latestPose.transform.rotation.x = q[0] latestPose.transform.rotation.y = q[1] latestPose.transform.rotation.z = q[2] latestPose.transform.rotation.w = q[3] self.interFrameMotions.append(latestPose) def publish(self): for i in self.interFrameMotions: i.header.stamp = rospy.Time.now() self.br.sendTransformMessage(i)
class TagTransformer(object): """ A TagTransformer is responsible for transforming detections by the apriltags_ros library into RoombaList messages for processing by the rest of the stack. """ def __init__(self, linear_covariance=0.1, angular_covariance=0.5, tf_frame='odom', camera_fov=1.0): """ :param linear_covariance: (meters) the linear covariance associated with a tag detection :param angular_covariance: (radians) the angular covariance of a tag :param tf_frame (str): The TF frame of the map / world. :param camera_fov (float): The FOV radius of the camera, in meters. """ self.tf = TransformListener() self.pub = rospy.Publisher('visible_roombas', RoombaList, queue_size=0) self.tf_pub = TransformBroadcaster() cov = [0] * 36 cov[0] = cov[7] = cov[14] = linear_covariance cov[21] = cov[28] = cov[35] = angular_covariance self.covariance = cov self.fov = camera_fov self.map_frame = tf_frame # Negates x and y coordinates of tag detections self.apply_apriltag_fix = True rospy.Subscriber('tag_detections', AprilTagDetectionArray, self.on_tags) def on_tags(self, msg): """ Handler for AprilTag detection. Converts AprilTagDetectionArray to a RoombaList object, then publishes it. :type msg: AprilTagDetectionArray """ sighting = RoombaList() for tag in msg.detections: # type: AprilTagDetection if self.apply_apriltag_fix: tag.pose.pose.position.x *= -1 tag.pose.pose.position.y *= -1 roomba_frame_id = 'roombas/{}'.format(tag.id) # Transform the detection into map frame, and constrain it to be flat on the ground pose = self.tf.transformPose(self.map_frame, tag.pose) pose.pose.position.z = 0 _, _, z_angle = euler_from_quaternion( [getattr(pose.pose.orientation, s) for s in 'xyzw']) pose.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, z_angle)) roomba = Roomba() roomba.visible_location = PoseWithCovarianceStamped( header=pose.header, pose=PoseWithCovariance(pose=pose.pose, covariance=self.covariance)) # Because April tags don't have color information, assign even tags to be GREEN and odd tags to be RED roomba.type = [Roomba.GREEN, Roomba.RED][tag.id % 2] roomba.last_seen = tag.pose.header.stamp roomba.frame_id = roomba_frame_id roomba.magical_id = tag.id + 100 sighting.data.append(roomba) # self.tf_pub.sendTransform(roomba.visible_location.pose.pose.position, # roomba.visible_location.pose.pose.orientation, # roomba.last_seen, roomba_frame_id, self.tf_frame) self.tf_pub.sendTransformMessage( TransformStamped( header=roomba.visible_location.header, child_frame_id=roomba.frame_id, transform=Transform( translation=roomba.visible_location.pose.pose.position, rotation=roomba.visible_location.pose.pose.orientation) )) self.pub.publish(sighting)
class RiCDiffCloseLoop(Device): def __init__(self, param, output): Device.__init__(self, param.getCloseDiffName(), output) self._baseLink = param.getCloseDiffBaseLink() self._odom = param.getCloseDiffOdom() self._maxAng = param.getCloseDiffMaxAng() self._maxLin = param.getCloseDiffMaxLin() self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz()) self._broadCase = TransformBroadcaster() Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1) Service('%s/setOdometry' % self._name, set_odom, self.setOdom) self._haveRightToPublish = False self._prevOdom = None self._firstTimePub = True def getType(self): return DiffDriverCL def diffServiceCallback(self, msg): Thread(target=self.sendMsg, args=(msg, )).start() def sendMsg(self, msg): if msg.angular.z > self._maxAng: msg.angular.z = self._maxAng elif msg.angular.z < -self._maxAng: msg.angular.z = -self._maxAng if msg.linear.x > self._maxLin: msg.linear.x = self._maxLin elif msg.linear.x < -self._maxLin: msg.linear.x = -self._maxLin # print msg.angular.z, msg.linear.x self._output.write( CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend()) def setOdom(self, req): self._output.write( CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend()) return set_odomResponse(True) def publish(self, data): q = Quaternion() q.x = 0 q.y = 0 q.z = data[6] q.w = data[7] odomMsg = Odometry() odomMsg.header.frame_id = self._odom odomMsg.header.stamp = rospy.get_rostime() odomMsg.child_frame_id = self._baseLink odomMsg.pose.pose.position.x = data[0] odomMsg.pose.pose.position.y = data[1] odomMsg.pose.pose.position.z = 0 odomMsg.pose.pose.orientation = q if self._firstTimePub: self._prevOdom = odomMsg self._firstTimePub = False return velocity = Twist() deltaTime = odomMsg.header.stamp.to_sec( ) - self._prevOdom.header.stamp.to_sec() yaw, pitch, roll = euler_from_quaternion([ odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y, odomMsg.pose.pose.orientation.z ]) prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion([ self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x, self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z ]) if deltaTime > 0: velocity.linear.x = (data[8] / deltaTime) deltaYaw = yaw - prevYaw # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw)) if deltaYaw > math.pi: deltaYaw -= 2 * math.pi elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi if deltaTime > 0: velocity.angular.z = -(deltaYaw / deltaTime) # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z)) odomMsg.twist.twist = velocity self._prevOdom = odomMsg traMsg = TransformStamped() traMsg.header.frame_id = self._odom traMsg.header.stamp = rospy.get_rostime() traMsg.child_frame_id = self._baseLink traMsg.transform.translation.x = data[0] traMsg.transform.translation.y = data[1] traMsg.transform.translation.z = 0 traMsg.transform.rotation = q self._pub.publish(odomMsg) self._broadCase.sendTransformMessage(traMsg) def checkForSubscribers(self): try: subCheck = re.search('Subscribers:.*', rostopic.get_info_text( self._pub.name)).group(0).split(': ')[1] subTfCheck = re.search( 'Subscribers:.*', rostopic.get_info_text('/tf')).group(0).split(': ')[1] if not self._haveRightToPublish and (subCheck == '' or subTfCheck == ''): self._output.write( PublishRequest(DiffDriverCL, 0, True).dataTosend()) self._haveRightToPublish = True elif self._haveRightToPublish and (subCheck == 'None' and subTfCheck == 'None'): self._output.write( PublishRequest(DiffDriverCL, 0, False).dataTosend()) self._haveRightToPublish = False self._firstTimePub = True except: pass
class OdomNode: def __init__(self): self.const = 0.05235987755 self.left_value = None self.rigth_value = None self.x_pos = 0.0 self.y_pos = 0.0 self.l_vel = 0.0 self.a_vel = 0.0 self.heading = 0 self.left_sub = rospy.Subscriber("/left_encoder_value", Int64, self.callback_left_encoder, buff_size=30) self.right_sub = rospy.Subscriber("/right_encoder_value", Int64, self.callback_right_encoder, buff_size=30) self.odom_publisher = rospy.Publisher("odom", Odometry, queue_size=50) self.last_time = rospy.Time.now() self.odom_broadcaster = TransformBroadcaster() def callback_left_encoder(self, msg): self.left_value = msg.data pass def callback_right_encoder(self, msg): self.right_value = msg.data self.publish_odometry() pass def publish_odometry(self): current_time = rospy.Time.now() omega_l = (self.left_value * self.const) omega_r = (self.right_value * self.const) self.l_vel = ((omega_l + omega_r) / 2.0) * 0.062 self.a_vel = -(omega_l - omega_r) * (0.062 / 0.45) vel_dt = (current_time - self.last_time).to_sec() delta_heading = self.a_vel * vel_dt delta_x = (self.l_vel * np.cos(self.heading)) * vel_dt delta_y = (self.l_vel * np.sin(self.heading)) * vel_dt #print(self.x_pos, self.y_pos, self.heading, self.l_vel, self.a_vel, self.left_value, self.right_value, vel_dt) self.x_pos += delta_x self.y_pos += delta_y self.heading += delta_heading # TF2 heading to quaternion quat = quaternion_from_euler(0, 0, self.heading) odom_trans = TransformStamped() odom_trans.header.stamp = current_time odom_trans.header.frame_id = "odom" odom_trans.child_frame_id = "base_link" odom_trans.transform.translation.x = self.x_pos odom_trans.transform.translation.y = self.y_pos odom_trans.transform.translation.z = 0.0 odom_trans.transform.rotation.x = quat[0] odom_trans.transform.rotation.y = quat[1] odom_trans.transform.rotation.z = quat[2] odom_trans.transform.rotation.w = quat[3] #self.odom_broadcaster.sendTransform(odom_trans) self.odom_broadcaster.sendTransformMessage(odom_trans) odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.pose.pose.position.x = self.x_pos odom.pose.pose.position.y = self.y_pos odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation.x = quat[0] odom.pose.pose.orientation.y = quat[1] odom.pose.pose.orientation.z = quat[2] odom.pose.pose.orientation.w = quat[3] odom.pose.covariance[0] = 0.0001 odom.pose.covariance[7] = 0.0001 odom.pose.covariance[35] = 0.0001 odom.twist.twist.linear.x = self.l_vel odom.twist.twist.linear.y = 0.0 odom.twist.twist.linear.z = 0.0 odom.twist.twist.angular.x = 0.0 odom.twist.twist.angular.y = 0.0 odom.twist.twist.angular.z = self.a_vel odom.twist.covariance[0] = 0.0001 odom.twist.covariance[7] = 0.0001 odom.twist.covariance[35] = 0.0001 self.odom_publisher.publish(odom) self.last_time = current_time
class RiCDiffCloseLoop(Device): def __init__(self, param, output): Device.__init__(self, param.getCloseDiffName(), output) self._baseLink = param.getCloseDiffBaseLink() self._odom = param.getCloseDiffOdom() self._maxAng = param.getCloseDiffMaxAng() self._maxLin = param.getCloseDiffMaxLin() self._pub = Publisher("%s/odometry" % self._name, Odometry, queue_size=param.getCloseDiffPubHz()) self._broadCase = TransformBroadcaster() Subscriber('%s/command' % self._name, Twist, self.diffServiceCallback, queue_size=1) Service('%s/setOdometry' % self._name, set_odom, self.setOdom) self._haveRightToPublish = False self._prevOdom = None self._firstTimePub = True def getType(self): return DiffDriverCL def diffServiceCallback(self, msg): Thread(target=self.sendMsg, args=(msg,)).start() def sendMsg(self, msg): if msg.angular.z > self._maxAng: msg.angular.z = self._maxAng elif msg.angular.z < -self._maxAng: msg.angular.z = -self._maxAng if msg.linear.x > self._maxLin: msg.linear.x = self._maxLin elif msg.linear.x < -self._maxLin: msg.linear.x = -self._maxLin # print msg.angular.z, msg.linear.x self._output.write(CloseDiffRequest(msg.angular.z, msg.linear.x).dataTosend()) def setOdom(self, req): self._output.write(CloseDiffSetOdomRequest(req.x, req.y, req.theta).dataTosend()) return set_odomResponse(True) def publish(self, data): q = Quaternion() q.x = 0 q.y = 0 q.z = data[6] q.w = data[7] odomMsg = Odometry() odomMsg.header.frame_id = self._odom odomMsg.header.stamp = rospy.get_rostime() odomMsg.child_frame_id = self._baseLink odomMsg.pose.pose.position.x = data[0] odomMsg.pose.pose.position.y = data[1] odomMsg.pose.pose.position.z = 0 odomMsg.pose.pose.orientation = q if self._firstTimePub: self._prevOdom = odomMsg self._firstTimePub = False return velocity = Twist() deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec() yaw, pitch, roll = euler_from_quaternion( [odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y, odomMsg.pose.pose.orientation.z]) prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion( [self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x, self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z]) if deltaTime > 0: velocity.linear.x = (data[8] / deltaTime) deltaYaw = yaw - prevYaw # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw)) if deltaYaw > math.pi: deltaYaw -= 2 * math.pi elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi if deltaTime > 0: velocity.angular.z = -(deltaYaw / deltaTime) # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z)) odomMsg.twist.twist = velocity self._prevOdom = odomMsg traMsg = TransformStamped() traMsg.header.frame_id = self._odom traMsg.header.stamp = rospy.get_rostime() traMsg.child_frame_id = self._baseLink traMsg.transform.translation.x = data[0] traMsg.transform.translation.y = data[1] traMsg.transform.translation.z = 0 traMsg.transform.rotation = q self._pub.publish(odomMsg) self._broadCase.sendTransformMessage(traMsg) def checkForSubscribers(self): try: subCheck = re.search('Subscribers:.*', rostopic.get_info_text(self._pub.name)).group(0).split(': ')[1] subTfCheck = re.search('Subscribers:.*', rostopic.get_info_text('/tf')).group(0).split(': ')[1] if not self._haveRightToPublish and (subCheck == '' or subTfCheck == ''): self._output.write(PublishRequest(DiffDriverCL, 0, True).dataTosend()) self._haveRightToPublish = True elif self._haveRightToPublish and (subCheck == 'None' and subTfCheck == 'None'): self._output.write(PublishRequest(DiffDriverCL, 0, False).dataTosend()) self._haveRightToPublish = False self._firstTimePub = True except: pass
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("/postImage",Image,queue_size=50) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback) self.h = Header() self.tb = TransformBroadcaster() self.img_tf=TransformStamped() def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) (rows,cols,channels) = cv_image.shape cv2.circle(cv_image, (320,240), 10, (0,255,255),-1) (corners, ids, rejected) = cv2.aruco.detectMarkers(cv_image,arucoDict, parameters=arucoParams) frame_markers = cv2.aruco.drawDetectedMarkers(cv_image.copy(), corners, ids) rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_size, mtx, dist) # print("trans",tvec) # print("rot",rvec) # cv2.aruco.drawAxis(cv_image,mtx,dist,rvec,tvec,0.1) cv2.aruco.drawAxis(frame_markers,mtx,dist,rvec,tvec,0.05) cv2.putText(frame_markers,np.array_str(tvec),(0,50),cv2.FONT_HERSHEY_SIMPLEX,0.7,(0,0,255),2) # cv2.imshow("Image window", frame_markers) # cv2.imshow("Image window", cv_image) # cv2.waitKey(3) self.tf_pub(tvec,rvec) try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame_markers, "bgr8")) except CvBridgeError as e: print(e) def tf_pub(self,tvec,rvec): # q=tf.quaternion_from_euler(0,-pi/2,-pi/2,axes="rxyz") # q=tf.quaternion_from_euler(pi/2,0,-pi/2,axes="sxyz") q=tf.quaternion_from_euler(rvec[0][0][0],rvec[0][0][1],rvec[0][0][2],axes="sxyz") print(rvec) self.img_tf.transform.translation.y=-tvec[0][0][0]; self.img_tf.transform.translation.z=-tvec[0][0][1]; self.img_tf.transform.translation.x=tvec[0][0][2]; # self.img_tf.transform.rotation.w=1; self.img_tf.transform.rotation.x=q[0];self.img_tf.transform.rotation.y=q[1]; self.img_tf.transform.rotation.z=q[2];self.img_tf.transform.rotation.w=q[3]; self.h.frame_id="cam_link" self.img_tf.header=self.h self.img_tf.child_frame_id="img_tf" self.h.stamp = rospy.Time.now() self.tb.sendTransformMessage(self.img_tf)
class moveit_interface: def __init__(self, config): rospy.loginfo("Start to intialize moveit_control_interface") # Config self.cfg = config.cfg # TF self.tf = TransformListener() self.br = TransformBroadcaster() rospy.Subscriber(self.cfg['topicMoveGroupResult'], MoveGroupActionResult, self.cb_move_group_result) rospy.Subscriber(self.cfg['topicTrajectoryExecutionResult'], ExecuteTrajectoryActionResult, self.cb_trajectory_execution_result) # Wait for Moveit action server moveit_interface_found = False while not moveit_interface_found: try: self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander( self.cfg['move_group_name']) moveit_interface_found = True except Exception as e: rospy.logerr(e.message) moveit_interface_found = False rospy.logerr("Retrying to connect to MoveIt action server ...") rospy.signal_shutdown('No MoveIt interface found') return # Create an inverse mapping of the joint names self.active_joints = self.group.get_active_joints() rospy.loginfo("============ Active joints: %d" % len(self.active_joints)) self.active_joints_id = {} i = 0 for joint_name in self.active_joints: self.active_joints_id[i] = joint_name i += 1 rospy.loginfo(self.active_joints_id) # Joint/Goal tolerances for motion planning: rospy.loginfo( "============ Goal tolerances: joint, position, orientation") self.group.set_goal_position_tolerance( self.cfg['goal_position_tolerance']) self.group.set_goal_joint_tolerance(self.cfg['goal_joint_tolerance']) self.group.set_goal_orientation_tolerance( self.cfg['goal_orientation_tolerance']) rospy.loginfo("Joint-, position-, orientation tolerance: ") rospy.loginfo( self.group.get_goal_tolerance() ) # Return a tuple of goal tolerances: joint, position and orientation. rospy.loginfo("============ Reference frame for poses of end-effector") rospy.loginfo(self.group.get_pose_reference_frame()) ## Publish a goal to TF def _publish_tf(self, poseStamped, name="moveit_target"): transform = TransformStamped() transform.header = poseStamped.header transform.transform.translation = poseStamped.pose.position transform.transform.rotation = poseStamped.pose.orientation transform.child_frame_id = name self.br.sendTransformMessage(transform) ## Init the MoveIt planning scene # Important: the moveit_commander needs some time to come up! def init_planning_scene(self, remove_objects=False, addGround=False): rospy.loginfo("Initializing the planning scene") rospy.sleep(2.0) if remove_objects: rospy.loginfo("Removing all objects from the planning scene") self.scene.remove_world_object() if addGround: if not self.add_ground("ground", 0.0): rospy.logerr("Ground was not added to the scene") def _add_object_decorator(func): def wrapper(*args, **kwargs): self = args[0] rospy.loginfo("Adding object '" + args[1] + "' to the scene.") func(*args, **kwargs) res = self.wait_for_object(args[1]) rospy.loginfo("Known objects in the scene: " + str(self.scene.get_known_object_names())) return res return wrapper ## Adds a horizontal ground plane to the planning scene # @param name Name of the ground plane # @param z z-Position of the ground plane @_add_object_decorator def add_ground(self, name, z=0.0, frame=""): p = geometry_msgs.msg.PoseStamped() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = z p.pose.orientation.w = 1.0 if frame is not "": p.header.frame_id = frame else: p.header.frame_id = self.robot.get_planning_frame() self.scene.add_plane(name, p) ## Add a box to the planning scene # @param name Name of the Object # @param size Size of the object given as vector list (dx,dy,dz) # @param position Position of the object given as vector list (x,y,z) # @param orientation Orientation of the object given as quaternion list (x,y,z,w) # @param frame Attach to the given frame @_add_object_decorator def add_box(self, name, size, position, orientation, frame=""): p = geometry_msgs.msg.PoseStamped() p.pose.position = geometry_msgs.msg.Point(position[0], position[1], position[2]) p.pose.orientation = geometry_msgs.msg.Quaternion( orientation[0], orientation[1], orientation[2], orientation[3]) if frame is not "": p.header.frame_id = frame else: p.header.frame_id = self.robot.get_planning_frame() self.scene.add_box(name, p, size) ## Wait for timeout s if an object appears in the planning scene # @param object_name Nome of the object to check/wait for # @param timeout Wait for timeout s def wait_for_object(self, object_name, timeout=3.0): start = rospy.get_time() seconds = rospy.get_time() while (seconds - start < timeout) and not rospy.is_shutdown(): # Test if we are in the expected state if object_name in self.scene.get_known_object_names(): return True # Sleep so that we give other threads time on the processor rospy.loginfo("Waiting for scene to be updated") rospy.sleep(0.5) seconds = rospy.get_time() rospy.logerr("Object '" + object_name + "' was not found in the scene") return False ## Execute the plan passed as parameter or, if the passed plan is None, execute the most recently planned trajectory. def execute(self, plan=None, wait=True): if plan is None: self.group.set_start_state_to_current_state() plan = self.group.plan() if len(plan.joint_trajectory.points) is 0: rospy.logerr( "Empty trajectory, nothing to execute! Probably IK did not find a solution." ) return self.group.execute(plan, wait) #self.group.go(wait=True) ## Change the current pose in the direction of the passed axis by the passed value. # This is a helper function for the move_x/y/z and rotate_y/y/z methods. # @param axis The axis to change (integer from 0 to 5 corresponding to the translation/rotation axis) # @param value The value to be added to the current pose value. def _shift_pose_target(self, axis, value, wait=True): try: self.group.shift_pose_target(axis, value) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move to a new 6D endeffector pose (geometry_msgs/Pose) def move_pose(self, pose, wait=True): rospy.loginfo("Received pose command") try: self.group.set_pose_target(pose) self._publish_tf(pose) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move to a new 3D endeffector position (geometry_msgs/Point) def move_position(self, position, wait=True): try: rospy.loginfo("Received position command") self.group.set_position_target( [position.x, position.y, position.z]) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move to a named (and pre-defined) fixed pose def move_fixed_pose(self, fixed_pose_name, wait=True): rospy.loginfo("Received fixed pose command: %s" % fixed_pose_name) try: self.group.set_named_target(fixed_pose_name) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move to a given joint state def move_joint_state(self, joint_state, wait=True): rospy.loginfo("Received joint state command") desired_joint_pos = None if len(joint_state.name) is not 0 and len(joint_state.name) is len( joint_state.position): desired_joint_pos = dict( zip(joint_state.name, joint_state.position)) elif len(joint_state.name) is 0 and len(joint_state.position) is len( self.active_joints): desired_joint_pos = joint_state.position else: rospy.logerr("Invalid joint state message received! %s" % joint_state) if desired_joint_pos is not None: try: self.group.set_joint_value_target(desired_joint_pos) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move the endeffector relative in the x-direction def move_x(self, move_by_value, wait=True): rospy.loginfo("Received move x command: %f" % move_by_value) self._shift_pose_target(0, move_by_value, wait) return ## Move the endeffector relative in the x-direction in a straight line def move_x_straigt(self, move_by_value, wait=True): rospy.loginfo("Received move x straight command: %f" % move_by_value) pose = self.group.get_current_pose().pose pose.position.x += move_by_value self.move_cartesian_path_to_pose(pose) return ## Move the endeffector relative in the y-direction def move_y(self, move_by_value, wait=True): rospy.loginfo("Received move y command: %f" % move_by_value) self._shift_pose_target(1, move_by_value, wait) return ## Move the endeffector relative in the y-direction in a straight line def move_y_straigt(self, move_by_value, wait=True): rospy.loginfo("Received move y straight command: %f" % move_by_value) pose = self.group.get_current_pose().pose pose.position.y += move_by_value self.move_cartesian_path_to_pose(pose) return ## Move the endeffector relative in the z-direction def move_z(self, move_by_value, wait=True): rospy.loginfo("Received move z command: %f" % move_by_value) self._shift_pose_target(2, move_by_value, wait) return ## Move the endeffector relative in the z-direction in a straight line def move_z_straigt(self, move_by_value, wait=True): rospy.loginfo("Received move z straight command: %f" % move_by_value) pose = self.group.get_current_pose().pose pose.position.z += move_by_value self.move_cartesian_path_to_pose(pose) return ## Relative rotation of the endeffector around the x-axis def rotate_x(self, rotate_by_value, wait=True): rospy.loginfo("Received rotate x command: %f" % rotate_by_value) self._shift_pose_target(3, rotate_by_value, wait) return ## Relative rotation of the endeffector around the y-axis def rotate_y(self, rotate_by_value, wait=True): rospy.loginfo("Received rotate y command: %f" % rotate_by_value) self._shift_pose_target(4, rotate_by_value, wait) return ## Relative rotation of the endeffector around the z-axis def rotate_z(self, rotate_by_value, wait=True): rospy.loginfo("Received rotate z command: %f" % rotate_by_value) self._shift_pose_target(5, rotate_by_value, wait) return ## Move the endeffector to a named TF pose def move_to_tf_pose(self, tf_frame_name, wait=True): rospy.loginfo("Received move to TF pose command: %s" % tf_frame_name.data) pose_reference_frame = self.group.get_pose_reference_frame() try: t = self.tf.getLatestCommonTime(pose_reference_frame, tf_frame_name.data) pos, rot = self.tf.lookupTransform(pose_reference_frame, tf_frame_name.data, t) self.group.set_pose_target(pos + rot) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move the endeffector to the position of a named TF pose def move_to_tf_position(self, tf_frame_name, wait=True): rospy.loginfo("Received move to TF position command: %s" % tf_frame_name.data) pose_reference_frame = self.group.get_pose_reference_frame() try: t = self.tf.getLatestCommonTime(pose_reference_frame, tf_frame_name.data) pos, rot = self.tf.lookupTransform(pose_reference_frame, tf_frame_name.data, t) self.group.set_position_target(pos) self.execute(wait=wait) except Exception as e: rospy.logerr(e.message) return ## Move the endeffector on a cartesian path (straight line) to the goal pose def move_cartesian_path_to_pose(self, pose, wait=True): try: rospy.loginfo("Received move pose with cartesian path") waypoints = [] current_pose = self.group.get_current_pose() waypoints.append(copy.deepcopy(current_pose.pose)) waypoints.append(copy.deepcopy(pose)) # waypoints to follow, eef_step, jump_threshold (plan, fraction) = self.group.compute_cartesian_path( waypoints, 0.01, 0.0) self.execute(plan, wait) except Exception as e: rospy.logerr(e.message) return ## Get the current pose of the robot def get_current_pose(self): try: return self.group.get_current_pose().pose except Exception as e: rospy.logerr(e.message) return None ## Callback function for move group planning action result feedback def cb_move_group_result(self, result): rospy.loginfo("Move group: %s (code %d)" % (result.status.text, result.status.status)) ## Callback function for trajectory execution action result feedback def cb_trajectory_execution_result(self, result): rospy.loginfo("Trajectory execution: %s (code %d)" % (result.status.text, result.status.status))