我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用geometry_msgs.msg.PoseStamped()。
def to_pose_stamped(self): pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "map" pose.pose.position.x = self.x pose.pose.position.y = self.y pose.pose.position.z = 0.25 quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta) pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] return pose
def ik_solve(limb, pos, orient): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { str(limb): PoseStamped(header=hdr, pose=Pose(position=pos, orientation=orient))} ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1 if resp.isValid[0]: limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints else: return Errors.RaiseNotReachable(pos)
def subscribePose(): rospy.Subscriber('/initialpose', PoseWithCovarianceStamped, getPoseData) # rospy.Subscriber('/move_base_simple/goal', PoseStamped, getPoseData) global background
def __init__(self): self.map = None self.start = None self.goal = None self.moves = [Move(0.1, 0), # forward Move(-0.1, 0), # back Move(0, 1.5708), # turn left 90 Move(0, -1.5708)] # turn right 90 self.robot = Robot(0.5, 0.5) self.is_working = False # Replace with mutex after all self.map_subscriber = rospy.Subscriber("map", OccupancyGrid, self.new_map_callback) self.start_subscriber = rospy.Subscriber("initialpose", PoseWithCovarianceStamped, self.new_start_callback) self.goal_subscriber = rospy.Subscriber("goal", PoseStamped, self.new_goal_callback) self.path_publisher = rospy.Publisher("trajectory", MarkerArray, queue_size=1) self.pose_publisher = rospy.Publisher("debug_pose", PoseStamped, queue_size=1) # what will be there. A module goes into variable. Isn't it too much memory consumption. Maybe I should assign function replan() to this variable? self.planner = planners.astar.replan
def updatePoseTopic(self, next_index, wait=True): planning_group = self.planning_groups_keys[self.current_planning_group_index] topics = self.planning_groups[planning_group] if next_index >= len(topics): self.current_eef_index = 0 elif next_index < 0: self.current_eef_index = len(topics) - 1 else: self.current_eef_index = next_index next_topic = topics[self.current_eef_index] rospy.loginfo("Changed controlled end effector to " + self.planning_groups_tips[planning_group][self.current_eef_index]) self.pose_pub = rospy.Publisher(next_topic, PoseStamped, queue_size=5) if wait: self.waitForInitialPose(next_topic) self.current_pose_topic = next_topic
def markerCB(self, msg): try: self.marker_lock.acquire() if not self.initialize_poses: return self.initial_poses = {} for marker in msg.markers: if marker.name.startswith("EE:goal_"): # resolve tf if marker.header.frame_id != self.frame_id: ps = PoseStamped(header=marker.header, pose=marker.pose) try: transformed_pose = self.tf_listener.transformPose(self.frame_id, ps) self.initial_poses[marker.name[3:]] = transformed_pose.pose except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, e): rospy.logerr("tf error when resolving tf: %s" % e) else: self.initial_poses[marker.name[3:]] = marker.pose #tf should be resolved finally: self.marker_lock.release()
def waitForInitialPose(self, next_topic, timeout=None): counter = 0 while not rospy.is_shutdown(): counter = counter + 1 if timeout and counter >= timeout: return False try: self.marker_lock.acquire() self.initialize_poses = True topic_suffix = next_topic.split("/")[-1] if self.initial_poses.has_key(topic_suffix): self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix]) self.initialize_poses = False return True else: rospy.logdebug(self.initial_poses.keys()) rospy.loginfo("Waiting for pose topic of '%s' to be initialized", topic_suffix) rospy.sleep(1) finally: self.marker_lock.release()
def __init__(self): rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pose_callback, queue_size=1) rospy.Subscriber('/marker_data', MarkerArray, self.marker_callback, queue_size=1) self.vision_position_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) self.offset_pub = rospy.Publisher('~offset', PointStamped, queue_size=1) self.position_fcu_pub = rospy.Publisher('~position/fcu', PoseStamped, queue_size=1) self.pose_pub = rospy.Publisher('~position/marker_map', PoseStamped, queue_size=1) self.vision_position_message = PoseStamped() self.position_fcu_message = PoseStamped() self.position_fcu_message.header.frame_id = 'vision_fcu' self.pose_message = PoseStamped() self.pose_message.header.frame_id = 'marker_map' self.marker_position_offset = PointStamped()
def __init__(self): self.topics = { "ball": {"topic": "environment/ball", "sub": None, "data": CircularState(), "type": CircularState}, "light": {"topic": "environment/light", "sub": None, "data": UInt8(), "type": UInt8}, "sound": {"topic": "environment/sound", "sub": None, "data": Float32(), "type": Float32}, "ergo_state": {"topic": "ergo/state", "sub": None, "data": CircularState(), "type": CircularState}, "joy1": {"topic": "sensors/joystick/1", "sub": None, "data": Joy(), "type": Joy}, "joy2": {"topic": "sensors/joystick/2", "sub": None, "data": Joy(), "type": Joy}, "torso_l_j": {"topic": "{}/joint_state".format("poppy_torso"), "sub": None, "data": JointState(), "type": JointState}, "torso_l_eef": {"topic": "{}/left_arm/end_effector_pose".format("poppy_torso"), "sub": None, "data": PoseStamped(), "type": PoseStamped}, "torso_r_eef": {"topic": "{}/right_arm/end_effector_pose".format("poppy_torso"), "sub": None, "data": PoseStamped(), "type": PoseStamped} } self.topics["ball"]["sub"] = rospy.Subscriber(self.topics["ball"]["topic"], self.topics["ball"]["type"], self.cb_ball) self.topics["light"]["sub"] = rospy.Subscriber(self.topics["light"]["topic"], self.topics["light"]["type"], self.cb_light) self.topics["sound"]["sub"] = rospy.Subscriber(self.topics["sound"]["topic"], self.topics["sound"]["type"], self.cb_sound) self.topics["ergo_state"]["sub"] = rospy.Subscriber(self.topics["ergo_state"]["topic"], self.topics["ergo_state"]["type"], self.cb_ergo) self.topics["joy1"]["sub"] = rospy.Subscriber(self.topics["joy1"]["topic"], self.topics["joy1"]["type"], self.cb_joy1) self.topics["joy2"]["sub"] = rospy.Subscriber(self.topics["joy2"]["topic"], self.topics["joy2"]["type"], self.cb_joy2) self.topics["torso_l_j"]["sub"] = rospy.Subscriber(self.topics["torso_l_j"]["topic"], self.topics["torso_l_j"]["type"], self.cb_torso_l_j) self.topics["torso_l_eef"]["sub"] = rospy.Subscriber(self.topics["torso_l_eef"]["topic"], self.topics["torso_l_eef"]["type"], self.cb_torso_l_eef) self.topics["torso_r_eef"]["sub"] = rospy.Subscriber(self.topics["torso_r_eef"]["topic"], self.topics["torso_r_eef"]["type"], self.cb_torso_r_eef)
def __init__(self): self.frame_id = rospy.get_param("~frame_id", "map") cov_x = rospy.get_param("~cov_x", 0.6) cov_y = rospy.get_param("~cov_y", 0.6) cov_z = rospy.get_param("~cov_z", 0.6) self.cov = self.cov_matrix(cov_x, cov_y, cov_z) self.ps_pub = rospy.Publisher( POSE_TOPIC, PoseStamped, queue_size=1) self.ps_cov_pub = rospy.Publisher( POSE_COV_TOPIC, PoseWithCovarianceStamped, queue_size=1) self.ps_pub_3d = rospy.Publisher( POSE_TOPIC_3D, PoseStamped, queue_size=1) self.ps_cov_pub_3d = rospy.Publisher( POSE_COV_TOPIC_3D, PoseWithCovarianceStamped, queue_size=1) self.last = None self.listener = tf.TransformListener() self.tag_range_topics = rospy.get_param("~tag_range_topics") self.subs = list() self.ranges = dict() self.tag_pos = dict() self.altitude = 0.0 self.last_3d = None for topic in self.tag_range_topics: self.subs.append(rospy.Subscriber(topic, Range, self.range_cb))
def publish_position(self, pos, ps_pub, ps_cov_pub, cov): x, y = pos[0], pos[1] if len(pos) > 2: z = pos[2] else: z = 0 ps = PoseStamped() ps_cov = PoseWithCovarianceStamped() ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.header.frame_id = self.frame_id ps.header.stamp = rospy.get_rostime() ps_cov.header = ps.header ps_cov.pose.pose = ps.pose ps_cov.pose.covariance = cov ps_pub.publish(ps) ps_cov_pub.publish(ps_cov)
def callback_state(self, data): for idx, cube in enumerate(data.name): self.cubes_state.setdefault(cube, [0] * 3) pose = self.cubes_state[cube] cube_init = self.CubeMap[cube]["init"] pose[0] = data.pose[idx].position.x + cube_init[0] pose[1] = data.pose[idx].position.y + cube_init[1] pose[2] = data.pose[idx].position.z + cube_init[2] # def add_cube(self, name): # p = PoseStamped() # p.header.frame_id = ros_robot.get_planning_frame() # p.header.stamp = rospy.Time.now() # # # p.pose = self._arm.get_random_pose().pose # p.pose.position.x = -0.18 # p.pose.position.y = 0 # p.pose.position.z = 0.046 # # q = quaternion_from_euler(0.0, 0.0, 0.0) # p.pose.orientation = Quaternion(*q) # ros_scene.add_box(name, p, (0.02, 0.02, 0.02)) # # def remove_cube(self, name): # ros_scene.remove_world_object(name)
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.2 p.pose.position.y = 0.0 p.pose.position.z = 0.1 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.005, 0.005, 0.005)) return p.pose
def _add_grasp_block_(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() # p.pose = self._arm.get_random_pose().pose p.pose.position.x = 0.021 p.pose.position.y = 0.008 p.pose.position.z = 0.2885 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.01)) return p.pose
def set_pose_target(self, pose, end_effector_link = ""): """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): ok = False if type(pose) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(pose.header.frame_id) ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link) self.set_pose_reference_frame(old) elif type(pose) is Pose: ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link) else: ok = self._g.set_pose_target(pose, end_effector_link) if not ok: raise MoveItCommanderException("Unable to set target pose") else: raise MoveItCommanderException("There is no end effector to set the pose for")
def place(self, object_name, location=None): """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" result = False if location is None: result = self._g.place(object_name) elif type(location) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(location.header.frame_id) result = self._g.place(object_name, conversions.pose_to_list(location.pose)) self.set_pose_reference_frame(old) elif type(location) is Pose: result = self._g.place(object_name, conversions.pose_to_list(location)) elif type(location) is PlaceLocation: result = self._g.place(object_name, conversions.msg_to_string(location)) else: raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object") return result
def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose
def _add_grasp_block_(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.09)) return p.pose
def publish_pose(pose, stamp=None, frame_id='camera'): msg = geom_msg.PoseStamped(); msg.header.frame_id = frame_id msg.header.stamp = stamp if stamp is not None else rospy.Time.now() tvec = pose.tvec x,y,z,w = pose.quat.to_xyzw() msg.pose.position = geom_msg.Point(tvec[0],tvec[1],tvec[2]) msg.pose.orientation = geom_msg.Quaternion(x,y,z,w) _publish_pose(msg)
def rtk_position_to_numpy(msg): if isinstance(msg, Odometry): p = msg.pose.pose.position return np.array([p.x, p.y, p.z]) elif isinstance(msg, PoseStamped) or isinstance(msg, NavSatFix): p = msg.pose.position return np.array([p.x, p.y, p.z]) else: raise ValueError
def rtk_orientation_to_numpy(msg): if isinstance(msg, Odometry): p = msg.pose.pose.orientation return np.array([p.x, p.y, p.z, p.w]) elif isinstance(msg, PoseStamped) or isinstance(msg, NavSatFix): p = msg.pose.orientation return np.array([p.x, p.y, p.z, p.w]) else: raise ValueError
def pose_to_list(pose): if isinstance(pose, PoseStamped): plist = [[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z], [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w]] return plist raise TypeError("ROSBridge.pose_to_list only accepts Path, got {}".format(type(pose)))
def path_last_point_to_numpy(path): if isinstance(path, Path): path = path.poses[-1] if isinstance(path, PoseStamped): return ROSBridge.pose_to_list(path) raise TypeError("ROSBridge.path_last_point_to_numpy only accepts Path or PoseStamped, got {}".format(type(path)))
def __init__(self): self.target_pose = PoseStamped() self.target_velocity = Twist() self.aim = Pose() self.kick_power = 0.0 self.dribble_power = 0.0 self.velocity_control_enable = False self.chip_enable = False self.navigation_enable = True self._MAX_KICK_POWER = 8.0 self._MAX_DRIBBLE_POWER = 8.0
def moveGripperTo(self, position, rollpitchyaw=[-np.pi, 0.0, 0.0], timeout=1, useInitGuess=False, wait=False, rightArm=True, ret=False): # Move using IK and joint trajectory controller # Attach new pose to a frame poseData = list(position) + list(rollpitchyaw) frameData = PyKDL.Frame() poseFrame = dh.array2KDLframe(poseData) poseFrame = dh.frameConversion(poseFrame, frameData) pose = dh.KDLframe2Pose(poseFrame) # Create a PoseStamped message and perform transformation to given frame ps = PoseStamped() ps.header.frame_id = self.frame ps.pose.position = pose.position ps.pose.orientation = pose.orientation ps = self.tf.transformPose(self.frame, ps) # Perform IK if rightArm: ikGoal = self.rightArmKdl.inverse(ps.pose, q_guess=self.initRightJointGuess, min_joints=self.rightJointLimitsMin, max_joints=self.rightJointLimitsMax) # ikGoal = self.rightArmKdl.inverse(ps.pose, q_guess=(self.initRightJointGuess if useInitGuess or self.currentRightJointPositions is None else self.currentRightJointPositions), min_joints=self.rightJointLimitsMin, max_joints=self.rightJointLimitsMax) else: ikGoal = self.leftArmKdl.inverse(ps.pose, q_guess=self.initLeftJointGuess, min_joints=self.leftJointLimitsMin, max_joints=self.leftJointLimitsMax) # ikGoal = self.leftArmKdl.inverse(ps.pose, q_guess=(self.initLeftJointGuess if useInitGuess or self.currentLeftJointPositions is None else self.currentLeftJointPositions), min_joints=self.leftJointLimitsMin, max_joints=self.leftJointLimitsMax) if ikGoal is not None: if not ret: self.moveToJointAngles(ikGoal, timeout=timeout, wait=wait, rightArm=rightArm) else: return ikGoal else: print 'IK failed'
def ref_cb(msg): global odom vehicle_pub.publish(PoseStamped(pose=msg.pose.pose, header=msg.header)) odom = msg
def __init__(self): NaoqiNode.__init__(self, 'naoqi_moveto_listener') self.connectNaoQi() self.listener = tf.TransformListener() self.goal_sub = rospy.Subscriber('move_base_simple/goal', PoseStamped, self.goalCB) self.cvel_sub = rospy.Subscriber('cmd_vel', Twist, self.cvelCB) # (re-) connect to NaoQI:
def get_position(self): """ Where are we... """ world = self.zarj.transform.lookup_transform('world', self.zarj.walk.lfname, rospy.Time()) chest_position = self.zarj.transform.lookup_transform( 'pelvis', 'torso', rospy.Time()).transform pose = PoseStamped() pose.header.frame_id = 'pelvis' pose.header.stamp = rospy.Time() pose.pose.position = world.transform.translation pose.pose.orientation = chest_position.rotation return pose
def turn_body_to(self, angle, wait=True): """ Turn the torso to a given angle as related to the pelvis Angles smaller than 110 or larger than 250 are unstable """ log("Turn body to {} degrees".format(angle)) chest_position = self.zarj.transform.lookup_transform( 'pelvis', 'torso', rospy.Time()).transform msg = ChestTrajectoryRosMessage() msg.unique_id = self.zarj.uid msg.execution_mode = msg.OVERRIDE euler = euler_from_quaternion((chest_position.rotation.w, chest_position.rotation.x, chest_position.rotation.y, chest_position.rotation.z)) qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2]) pose = PoseStamped() pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0]) pose.header.frame_id = 'pelvis' pose.header.stamp = rospy.Time() result = self.zarj.transform.tf_buffer.transform(pose, 'world') point = SO3TrajectoryPointRosMessage() point.time = 1.0 point.orientation = result.pose.orientation point.angular_velocity = Vector3(0.0, 0.0, 0.0) msg.taskspace_trajectory_points = [point] self.chest_publisher.publish(msg) if wait: rospy.sleep(point.time + 0.1)
def __init__(self): self.initial_poses = {} self.planning_groups_tips = {} self.tf_listener = tf.TransformListener() self.marker_lock = threading.Lock() self.prev_time = rospy.Time.now() self.counter = 0 self.history = StatusHistory(max_length=10) self.pre_pose = PoseStamped() self.pre_pose.pose.orientation.w = 1 self.current_planning_group_index = 0 self.current_eef_index = 0 self.initialize_poses = False self.initialized = False self.parseSRDF() self.plan_group_pub = rospy.Publisher('/rviz/moveit/select_planning_group', String, queue_size=5) self.updatePlanningGroup(0) self.updatePoseTopic(0, False) self.joy_pose_pub = rospy.Publisher("/joy_pose", PoseStamped, queue_size=1) self.plan_pub = rospy.Publisher("/rviz/moveit/plan", Empty, queue_size=5) self.execute_pub = rospy.Publisher("/rviz/moveit/execute", Empty, queue_size=5) self.update_start_state_pub = rospy.Publisher("/rviz/moveit/update_start_state", Empty, queue_size=5) self.update_goal_state_pub = rospy.Publisher("/rviz/moveit/update_goal_state", Empty, queue_size=5) self.interactive_marker_sub = rospy.Subscriber("/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full", InteractiveMarkerInit, self.markerCB, queue_size=1) self.sub = rospy.Subscriber("/joy", Joy, self.joyCB, queue_size=1)
def __init__(self): self.bridge = CvBridge() self.x = 0.0 self.y = 0.0 self.ang = 0.0 self.pos_sub = rospy.Subscriber("/slam_out_pose", PoseStamped, self.pos_callback) self.image_sub = rospy.Subscriber("/map_image/full",Image,self.img_callback)
def pick(self, pose, direction=(0, 0, 1), distance=0.1): pregrasp_pose = self.translate(pose, direction, distance) while True: try: #stamped_pose = PoseStamped( Header(0, rospy.Time(0), n.robot.base), pose) self.move_ik(pregrasp_pose) break except AttributeError: print("can't find valid pose for gripper cause I'm dumb") return False rospy.sleep(0.5) # We want to block end effector opening so that the next # movement happens with the gripper fully opened. #self.gripper.open() while True: #rospy.loginfo("Going down to pick (at {})".format(self.tip.max())) if(not self.sensors_updated()): return if self.tip.max() > 2000: break else: scaled_direction = (di / 100 for di in direction) #print("Scaled direction: ", scaled_direction) v_cartesian = self._vector_to(scaled_direction) v_cartesian[2] = -.01 #print("cartesian: ", v_cartesian) v_joint = self.compute_joint_velocities(v_cartesian) #print("joint : ", v_joint) self.limb.set_joint_velocities(v_joint) rospy.loginfo('Went down!')
def move_calib_position(self): # move arm to the calibration position self.calib_pose = PoseStamped( Header(0, rospy.Time(0), self.robot.base), Pose(Point(0.338520675898,-0.175860479474,0.0356775075197), Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013))) self.robot.move_ik(self.calib_pose)
def imarker_callback(self, msg): # http://docs.ros.org/jade/api/visualization_msgs/html/msg/InteractiveMarkerFeedback.html # noqa rospy.loginfo('imarker_callback called') name = msg.marker_name new_pose = msg.pose self.int_markers[name] = PoseStamped(msg.header, new_pose)
def translate(self, pose, direction, distance): """Get an PoseStamped msg and apply a translation direction * distance """ # Let's get the pose, move it to the tf frame, and then # translate it base_pose = self.tl.transformPose(self.base, pose) base_pose.pose.position = Point(*np.array(direction) * distance + Point2list(base_pose.pose.position)) return base_pose
def move_ik(self, stamped_pose): """Take a PoseStamped and move the arm there. """ action_address = ('/' + self.robot_type + '_driver/pose_action/tool_pose') self.client = actionlib.SimpleActionClient( action_address, kinova_msgs.msg.ArmPoseAction) if not isinstance(stamped_pose, PoseStamped): raise TypeError("No duck typing here? :(") pose = stamped_pose.pose position, orientation = pose.position, pose.orientation # Send a cartesian goal to the action server. Adapted from kinova_demo rospy.loginfo("Waiting for SimpleAction server") self.client.wait_for_server() goal = kinova_msgs.msg.ArmPoseGoal() goal.pose.header = Header(frame_id=(self.robot_type + '_link_base')) goal.pose.pose.position = position goal.pose.pose.orientation = orientation rospy.loginfo("Sending goal") print goal self.client.send_goal(goal) # rospy.loginfo("Waiting for up to {} s for result".format(t)) if self.client.wait_for_result(rospy.Duration(100)): rospy.loginfo("Action succeeded") print self.client.get_result() return self.client.get_result() else: self.client.cancel_all_goals() rospy.loginfo(' the cartesian action timed-out') return None
def make_pose_stamped_msg(self,translation,orientation,frame='/j2n6a300_link_base'): msg = PoseStamped() msg.header.frame_id = frame msg.header.stamp = rospy.Time.now() msg.pose.position.x = translation[0] msg.pose.position.y = translation[1] msg.pose.position.z = translation[2] msg.pose.orientation.x = orientation[0] msg.pose.orientation.y = orientation[1] msg.pose.orientation.z = orientation[2] return msg
def make_pose_msg(position, orientation, timestamp): pose_msg = PoseStamped() pose_msg.header.stamp = timestamp pose_msg.header.frame_id = '/dvs_simulator' pose_msg.pose.position.x = position[0] pose_msg.pose.position.y = position[1] pose_msg.pose.position.z = position[2] pose_msg.pose.orientation.x = orientation[0] pose_msg.pose.orientation.y = orientation[1] pose_msg.pose.orientation.z = orientation[2] pose_msg.pose.orientation.w = orientation[3] return pose_msg
def visualize(self): ''' Publish various visualization messages. ''' if not self.DO_VIZ: return if self.pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray): # Publish the inferred pose for visualization ps = PoseStamped() ps.header = Utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) self.pose_pub.publish(ps) if self.particle_pub.get_num_connections() > 0: # publish a downsampled version of the particle distribution to avoid a lot of latency if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) self.publish_particles(self.particles[proposal_indices,:]) else: self.publish_particles(self.particles) if self.pub_fake_scan.get_num_connections() > 0 and isinstance(self.ranges, np.ndarray): # generate the scan from the point of view of the inferred position for visualization self.viz_queries[:,0] = self.inferred_pose[0] self.viz_queries[:,1] = self.inferred_pose[1] self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2] self.range_method.calc_range_many(self.viz_queries, self.viz_ranges) self.publish_scan(self.downsampled_angles, self.viz_ranges)
def __init__(self): """Set up class variables, initialize broadcaster and start subscribers.""" # Create broadcasters self.br_head = tf.TransformBroadcaster() self.br_arm = tf.TransformBroadcaster() # Create subscribers rospy.Subscriber("bax_head/pose", PoseStamped, self.head_callback, queue_size=1) rospy.Subscriber("bax_arm/pose", PoseStamped, self.arm_callback, queue_size=1) # Main while loop. rate = rospy.Rate(50) while not rospy.is_shutdown(): rate.sleep()
def __init__(self): """Set up class variables, initialize broadcaster and start subscribers.""" # Create broadcasters self.br_head = tf.TransformBroadcaster() self.br_rods = tf.TransformBroadcaster() # Create subscribers rospy.Subscriber("bax_head/pose", PoseStamped, self.head_callback, queue_size=1) rospy.Subscriber("rods/pose", PoseStamped, self.rod_callback, queue_size=1) # Main while loop. rate = rospy.Rate(50) while not rospy.is_shutdown(): rate.sleep()
def getAnglesFromPose(self,pose): if type(pose)==Pose: goal=PoseStamped() goal.header.frame_id="/base" goal.pose=pose else: goal=pose if not self.ik: rospy.logerror("ERROR: Inverse Kinematics service was not loaded") return None goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z) ikreq = SolvePositionIKRequest() ikreq.pose_stamp.append(goal) try: resp = self.iksvc(ikreq) if (resp.isValid[0]): return resp.joints[0] else: rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr) return None except rospy.ServiceException,e : rospy.loginfo("Service call failed: %s" % (e,)) return None
def __init__(self): self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") self.trajectory = LineTrajectory("/built_trajectory") # receive either goal points or clicked points self.publish_point_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_point_callback, queue_size=1) self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.clicked_point_callback, queue_size=1) # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory)
def clicked_point_callback(self, msg): if isinstance(msg, PointStamped): self.trajectory.addPoint(msg.point) elif isinstance(msg, PoseStamped): self.trajectory.addPoint(msg.pose.position) # publish visualization of the path being built self.trajectory.publish_viz()
def visualize(self): ''' Publish various visualization messages. ''' if not self.DO_VIZ: return if self.pose_pub.get_num_connections() > 0 and isinstance(self.inferred_pose, np.ndarray): ps = PoseStamped() ps.header = Utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) self.pose_pub.publish(ps) if self.particle_pub.get_num_connections() > 0: if self.MAX_PARTICLES > self.MAX_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) self.publish_particles(self.particles[proposal_indices,:]) else: self.publish_particles(self.particles) if self.pub_fake_scan.get_num_connections() > 0 and isinstance(self.ranges, np.ndarray): # generate the scan from the point of view of the inferred position for visualization self.viz_queries[:,0] = self.inferred_pose[0] self.viz_queries[:,1] = self.inferred_pose[1] self.viz_queries[:,2] = self.downsampled_angles + self.inferred_pose[2] self.range_method.calc_range_many(self.viz_queries, self.viz_ranges) self.publish_scan(self.downsampled_angles, self.viz_ranges)
def SendGoal(GoalPublisher, goal, time_stamp): # goal: [x, y, yaw] GoalMsg = PoseStamped() #GoalMsg.header.seq = 0 GoalMsg.header.stamp = time_stamp GoalMsg.header.frame_id = 'map' GoalMsg.pose.position.x = goal[0] GoalMsg.pose.position.y = goal[1] #GoalMsg.pose.position.z = 0.0 quaternion = quaternion_from_euler(0, 0, goal[2]) GoalMsg.pose.orientation.x = quaternion[0] GoalMsg.pose.orientation.y = quaternion[1] GoalMsg.pose.orientation.z = quaternion[2] GoalMsg.pose.orientation.w = quaternion[3] GoalPublisher.publish(GoalMsg)
def _Land(self, req): rospy.loginfo("Landing requested!") self._cf_state = 'land' return () # This callback function puts the target PoseStamped message into a local variable.
def __init__(self): # initialize ROS node rospy.init_node('target_detector', anonymous=True) # initialize publisher for target pose, PoseStamped message, and set initial sequence number self.pub = rospy.Publisher('target_pose', PoseStamped, queue_size=1) self.pub_pose = PoseStamped() self.pub_pose.header.seq = 0 self.rate = rospy.Rate(1.0) # publish message at 1 Hz # initialize values for locating target on Kinect v2 image self.target_u = 0 # u is pixels left(0) to right(+) self.target_v = 0 # v is pixels top(0) to bottom(+) self.target_d = 0 # d is distance camera(0) to target(+) from depth image self.target_found = False # flag initialized to False self.last_d = 0 # last non-zero depth measurement # target orientation to Kinect v2 image (Euler) self.r = 0 self.p = 0 self.y = 0 # Convert image from a ROS image message to a CV image self.bridge = CvBridge() # Wait for the camera_info topic to become available rospy.wait_for_message('/kinect2/qhd/image_color_rect', Image) # Subscribe to registered color and depth images rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1) rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1) self.rate.sleep() # suspend until next cycle # This callback function handles processing Kinect color image, looking for the pink target.
def depth_callback(self, msg): # process only if target is found if self.target_found == True: # create OpenCV depth image using default passthrough encoding try: depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') except CvBridgeError as e: print(e) # using target (v, u) location, find depth value of point and divide by 1000 # to change millimeters into meters (for Kinect sensors only) self.target_d = depth_image[self.target_v, self.target_u] / 1000.0 # if depth value is zero, use the last non-zero depth value if self.target_d == 0: self.target_d = self.last_d else: self.last_d = self.target_d # record target location and publish target pose message rospy.loginfo("Target depth: x at %d y at %d z at %f", int(self.target_u), int(self.target_v), self.target_d) self.update_target_pose (self.target_u, self.target_v, self.target_d) # This function builds the target PoseStamped message and publishes it.
