我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用geometry_msgs.msg.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 GetWayPoints(self,Data): filename = Data.data #clear all existing marks in rviz for marker in self.makerArray.markers: marker.action = Marker.DELETE self.makerArray_pub.publish(self.makerArray) # clear former lists self.waypoint_list.clear() self.marker_list[:] = [] self.makerArray.markers[:] = [] f = open(filename,'r') for line in f.readlines(): j = line.split(",") current_wp_name = str(j[0]) current_point = Point(float(j[1]),float(j[2]),float(j[3])) current_quaternion = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7])) self.waypoint_list[current_wp_name] = Pose(current_point,current_quaternion) f.close() self.create_markers() self.makerArray_pub.publish(self.makerArray)
def observationToPose(self, observation): pose = geometry_msgs.msg.Pose() if observation['orientation'] == None: # if observation is ball observation['orientation'] = 0.0 if self.do_side_invert == True: observation['x'] = -observation['x'] observation['y'] = -observation['y'] observation['orientation'] = observation['orientation'] + math.pi pose.position.x = observation['x'] / 1000 pose.position.y = observation['y'] / 1000 pose.position.z = observation['z'] / 1000 quat = quaternion_from_euler(0.0, 0.0, observation['orientation']) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
def spawn_model(model_name): """ Spawns a model in front of the RGBD camera. Args: None Returns: None """ initial_pose = Pose() initial_pose.position.x = 0 initial_pose.position.y = 1 initial_pose.position.z = 1 # Spawn the new model # model_path = rospkg.RosPack().get_path('sensor_stick')+'/models/' model_xml = '' with open (model_path + model_name + '/model.sdf', 'r') as xml_file: model_xml = xml_file.read().replace('\n', '') spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) spawn_model_prox('training_model', model_xml, '', initial_pose, 'world')
def __init__(self, image_path=None): height = int(rospy.get_param("~grid_height", 800)) width = int(rospy.get_param("~grid_width", 800)) resolution = rospy.get_param("~grid_resolution", .25) ogrid_topic = rospy.get_param("/lqrrt_node/ogrid_topic", "/ogrid") self.grid_drawer = DrawGrid(height, width, image_path) self.ogrid_pub = rospy.Publisher(ogrid_topic, OccupancyGrid, queue_size=1) m = MapMetaData() m.resolution = resolution m.width = width m.height = height pos = np.array([-width * resolution / 2, -height * resolution / 2, 0]) quat = np.array([0, 0, 0, 1]) m.origin = Pose() m.origin.position.x, m.origin.position.y = pos[:2] self.map_meta_data = m rospy.Timer(rospy.Duration(1), self.pub_grid)
def __init__(self, limb_name, topic='/sensor_values'): super(FingerSensorBaxter, self).__init__(limb_name) self.listen = tf.TransformListener() self.inside = np.zeros(14) self.tip = np.zeros(2) self.inside_offset = np.zeros_like(self.inside) self.tip_offset = np.zeros_like(self.tip) # Picking level self.level = None self.last_sensor_update = 0 self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.update_sensor_values, queue_size=1) self.object_frame = "" self.camera_centroids = np.zeros((num_cubes, 3)) self.touch_centroids = np.zeros((num_cubes, 3)) self.current_index = 0 self.update_transform = rospy.Publisher("/update_camera_alignment", Pose, queue_size = 1) #self.zero_sensor()
def update_translation(self): t = self.tl.getLatestCommonTime("/root", "/camera_link") position, quaternion = self.tl.lookupTransform("/root", "/camera_link", t) print("Cam Pos:") print(position) translation = self.touch_centroids[0] - self.camera_centroids[0] print("Translation: ") print(translation) position = position + translation print("New Cam Pos:") print(position) #print(self.touch_centroids) #print(self.camera_centroids) self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))
def __init__(self, limb_name, topic='/sensor_values'): super(FingerSensorBaxter, self).__init__(limb_name) self.listen = tf.TransformListener() self.inside = np.zeros(14) self.tip = np.zeros(2) self.inside_offset = np.zeros_like(self.inside) self.tip_offset = np.zeros_like(self.tip) # Picking level self.level = None self.last_sensor_update = 0 self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.update_sensor_values, queue_size=1) self.object_frame = "" self.perc_centroids = np.array([]) self.touch_centroids = np.array([]) self.update_transform = rospy.Publisher("/update_camera_alignment", Pose, queue_size = 1) self.handle_found = False #self.zero_sensor() #import ipdb;ipdb.set_trace()
def position(self, target_position, trans, height): """ Calculate simple position of the robot's arm. Args: target_position (Pose): Wanted coordinates of robot's tool trans: Calculated transformation height (float): Height offset, depends on the number of disks on the rod Returns: target_position (Pose): Modified coordinates and orientation of robot's tool """ roll = -math.pi / 2 pitch = 0 yaw = -math.pi / 2 quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) target_position.orientation.x = quaternion[0] target_position.orientation.y = quaternion[1] target_position.orientation.z = quaternion[2] target_position.orientation.w = quaternion[3] target_position.position.x = trans[0] target_position.position.y = trans[1] target_position.position.z = trans[2] + height return target_position
def marker_from_circle(circle, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Markers_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = circle.center[0] marker.pose.position.y = circle.center[1] marker.scale = Vector3(circle.radius*2.0, circle.radius*2.0, 0) return marker
def marker_from_point_radius(point, radius, index=0, linewidth=0.1, color=ColorRGBA(1, 0, 0, 1), z=0., lifetime=10.0): marker = Marker() marker.header = make_header("/map") marker.ns = "Speed_NS" marker.id = index marker.type = Marker.CYLINDER marker.action = 0 # action=0 add/modify object # marker.color.r = 1.0 # marker.color.g = 0.0 # marker.color.b = 0.0 # marker.color.a = 0.4 marker.color = color marker.lifetime = rospy.Duration.from_sec(lifetime) marker.pose = Pose() marker.pose.position.z = z marker.pose.position.x = point[0] marker.pose.position.y = point[1] marker.scale = Vector3(radius*2.0, radius*2.0, 0.001) return marker
def list_to_pose(pose_list): pose_msg = Pose() if len(pose_list) == 7: pose_msg.position.x = pose_list[0] pose_msg.position.y = pose_list[1] pose_msg.position.z = pose_list[2] pose_msg.orientation.x = pose_list[3] pose_msg.orientation.y = pose_list[4] pose_msg.orientation.z = pose_list[5] pose_msg.orientation.w = pose_list[6] elif len(pose_list) == 6: pose_msg.position.x = pose_list[0] pose_msg.position.y = pose_list[1] pose_msg.position.z = pose_list[2] q = tf.transformations.quaternion_from_euler(pose_list[3], pose_list[4], pose_list[5]) pose_msg.orientation.x = q[0] pose_msg.orientation.y = q[1] pose_msg.orientation.z = q[2] pose_msg.orientation.w = q[3] else: raise MoveItCommanderException("Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)") return pose_msg
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 go(self, joints = None, wait = True): """ Set the target of the group and then move the group to the specified target """ if type(joints) is bool: wait = joints joints = None elif type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) elif not joints == None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except: self.set_joint_value_target(joints) if wait: return self._g.move() else: return self._g.async_move()
def plan(self, joints = None): """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """ if type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) elif not joints == None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except: self.set_joint_value_target(joints) plan = RobotTrajectory() plan.deserialize(self._g.compute_plan()) return plan
def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg)
def test_pose(self): t = Pose( position=Point(1.0, 2.0, 3.0), orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0)) ) t_mat = ros_numpy.numpify(t) np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0]) msg = ros_numpy.msgify(Pose, t_mat) np.testing.assert_allclose(msg.position.x, t.position.x) np.testing.assert_allclose(msg.position.y, t.position.y) np.testing.assert_allclose(msg.position.z, t.position.z) np.testing.assert_allclose(msg.orientation.x, t.orientation.x) np.testing.assert_allclose(msg.orientation.y, t.orientation.y) np.testing.assert_allclose(msg.orientation.z, t.orientation.z) np.testing.assert_allclose(msg.orientation.w, t.orientation.w)
def numpy_to_pose(arr): from tf import transformations shape, rest = arr.shape[:-2], arr.shape[-2:] assert rest == (4,4) if len(shape) == 0: trans = transformations.translation_from_matrix(arr) quat = transformations.quaternion_from_matrix(arr) return Pose( position=Vector3(*trans), orientation=Quaternion(*quat) ) else: res = np.empty(shape, dtype=np.object_) for idx in np.ndindex(shape): res[idx] = Pose( position=Vector3(*transformations.translation_from_matrix(arr[idx])), orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx])) )
def spawn_sdf(gazebo_name, path_to_sdf, position, orientation, static): try: with open (path_to_sdf, "r") as f: xml=f.read().replace('\n', '') if static: root = ET.fromstring(xml) #parse the xml and retrieve the root element (sdf tag)* static_tag = ET.Element(tag='static') static_tag.text='true' root.find('model').append(static_tag) # find the model tag and insert a static child xml = ET.tostring(root) # get the new xml print(xml) GazeboObject.spawn_sdf_srv.wait_for_service() resp_spawn = GazeboObject.spawn_sdf_srv(gazebo_name, xml, "/", Pose(position=position, orientation=orientation), "world") if resp_spawn.success: rospy.loginfo("creation of "+ gazebo_name + " successful") return True else: rospy.logerr("Could not spawn "+path_to_sdf+" (from "+path_to_sdf+"), status : "+resp_spawn.status_message) return False except Exception as e: rospy.logerr("Could not spawn "+path_to_sdf+" (from "+path_to_sdf+")" ) rospy.logerr(e) return False
def geom_pose_from_rt(rt): msg = geom_msg.Pose() msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2]) xyzw = rt.quat.to_xyzw() msg.orientation = geom_msg.Quaternion(x=xyzw[0], y=xyzw[1], z=xyzw[2], w=xyzw[3]) return msg
def GetWayPoints(self,Data): # dir = os.path.dirname(__file__) # filename = dir+'/locationPoint.txt' filename = Data.data rospy.loginfo(str(filename)) rospy.loginfo(self.locations) self.locations.clear() rospy.loginfo(self.locations) f = open(filename,'r') for i in f.readlines(): j = i.split(",") current_wp_name = str(j[0]) rospy.loginfo(current_wp_name) current_point = Point(float(j[1]),float(j[2]),float(j[3])) current_quaternion = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7])) self.locations[current_wp_name] = Pose(current_point,current_quaternion) f.close() rospy.loginfo(self.locations) #Rviz Marker self.init_markers() self.IsWPListOK = True
def msgToPose(self, detection_pose): pose = Pose() pose.position.x = detection_pose.x / 1000 pose.position.y = detection_pose.y / 1000 if hasattr(detection_pose, 'orientation'): quat_tuple = quaternion_from_euler(0.0, 0.0, detection_pose.orientation) pose.orientation = Quaternion(*quat_tuple) return pose
def __init__(self, limb_name, topic='/sensor_values'): super(FingerSensorBaxter, self).__init__(limb_name) self.listen = tf.TransformListener() self.inside = np.zeros(14) self.tip = np.zeros(2) self.inside_offset = np.zeros_like(self.inside) self.tip_offset = np.zeros_like(self.tip) # Picking level self.level = None self.last_sensor_update = 0 self.sensor_sub = rospy.Subscriber(topic, Int32MultiArray, self.update_sensor_values, queue_size=1) self.object_frame = "" self.perc_centroids = np.array([]) self.touch_centroids = np.array([]) self.update_transform = rospy.Publisher("/update_camera_alignment", Pose, queue_size = 1) self.handle_found = False self.handles_found = 0 self.cups_scanned = 0 #self.zero_sensor() #import ipdb;ipdb.set_trace()
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 _pick(self): # State not modified until picking succeeds try: obj_to_get = int(self.character) except ValueError: rospy.logerr("Please provide a number in picking mode") return frame_name = "object_pose_{}".format(obj_to_get) rospy.loginfo("Picking object {}...".format(obj_to_get)) if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name): t = self.tl.getLatestCommonTime(self.robot.base, frame_name) position, quaternion = self.tl.lookupTransform(self.robot.base, frame_name, t) print("position", position) print("quaternion", quaternion) position = list(position) # Vertical orientation self.br.sendTransform(position, [1, 0, 0, 0], rospy.Time.now(), "pick_pose", self.robot.base) # Ignore orientation from perception pose = Pose(Point(*position), Quaternion(1, 0, 0, 0)) h = Header() h.stamp = t h.frame_id = self.robot.base stamped_pose = PoseStamped(h, pose) self.robot.pick(stamped_pose) self.state = 'postpick'
def make_empty_pose(): return Pose( Point(0, 0, 0), Quaternion(0, 0, 0, 1) )
def particle_to_pose(particle): ''' Converts a particle in the form [x, y, theta] into a Pose object ''' pose = Pose() pose.position.x = particle[0] pose.position.y = particle[1] pose.orientation = angle_to_quaternion(particle[2]) return pose
def __init__(self, max_random_start, observation_dims, data_format, display, use_cumulated_reward): self.max_random_start = max_random_start self.action_size = 28 self.use_cumulated_reward = use_cumulated_reward self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) #self.dirToTarget = 0 self.robotPose = Pose() self.goalPose = Pose() self.robotRot = 0.0 self.prevDist = 0.0 self.numWins = 0 self.ep_reward = 0.0 self.readyForNewData = True self.terminal = 0 self.sendTerminal = 0 self.minFrontDist = 6 self.r = 1 self.ang = 0 rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Subscribers: rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10) # publishers: self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10) self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10) self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)
def __init__(self, max_random_start, observation_dims, data_format, display, use_cumulated_reward): self.max_random_start = max_random_start self.action_size = 28 self.use_cumulated_reward = use_cumulated_reward self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) #self.dirToTarget = 0 self.robotPose = Pose() self.goalPose = Pose() self.robotRot = 0.0 self.prevDist = 0.0 self.numWins = 0 self.ep_reward = 0.0 self.readyForNewData = True self.terminal = 0 self.sendTerminal = 0 self.minFrontDist = 6 rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Subscribers: rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10) # publishers: self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10) self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10) self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)
def close_gripper(self): """Send the instruction to the robot to close the gripper.""" goal = Pose() goal_final = baxterGoal(id=3, pose=goal) status = self.left_client.send_goal_and_wait(goal_final) result = self.left_client.wait_for_result() return result
def open_gripper(self): """Send the instruction to the robot to open the gripper.""" goal = Pose() goal_final = baxterGoal(id=2, pose=goal) self.left_client.send_goal_and_wait(goal_final) result = self.left_client.wait_for_result() return result
def test_relative(self): """Test robot's ability to position its gripper relative to a given marker.""" goal = Pose() roll = -math.pi / 2 pitch = 0 yaw = -math.pi / 2 quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) goal.orientation.x = quaternion[0] goal.orientation.y = quaternion[1] goal.orientation.z = quaternion[2] goal.orientation.w = quaternion[3] while True: end = user_input("Zelite li nastaviti? d/n") if end != 'd': break trans = self.transformations() goal.position.x = trans[0] goal.position.y = trans[1] goal.position.z = trans[2] offset_x = float(user_input("X?")) offset_y = float(user_input("Y?")) offset_z = float(user_input("Z?")) # Uncomment for testing movement speed as well # brzina = float(user_input("Brzina?")) # self.left_arm.set_joint_position_speed(brzina) goal.position.x += offset_x goal.position.y += offset_y goal.position.z += offset_z goal_final = baxterGoal(id=1, pose=goal) self.left_client.send_goal_and_wait(goal_final) result = self.left_client.get_result()
def __init__(self): rospy.init_node("speech") rospy.on_shutdown(self.shutdown) self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait up to 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) nav_goal = MoveBaseGoal() nav_goal.target_pose.header.frame_id = 'base_link' nav_goal.target_pose.header.stamp = rospy.Time.now() q_angle = quaternion_from_euler(0, 0,0, axes='sxyz') q = Quaternion(*q_angle) nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q) move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, exec_timeout=rospy.Duration(60.0), server_wait_timeout=rospy.Duration(60.0)) smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"]) with smach_top: StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb), transitions={'invalid':'NAV', 'valid':'Wait_4_Statr', 'preempted':'NAV'}) StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"}) StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb), transitions={'invalid':'', 'valid':'Wait_4_Stop', 'preempted':''}) smach_top.execute()
def people_msg_cb(self,msg): self.people_msg=msg if self.people_msg.pos is not None: goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'base_link' q_angle = quaternion_from_euler(0, 0,0) self.q=Quaternion(*q_angle) goal.target_pose.pose=(Pose(Point(self.people_msg.pos.x-0.5,self.people_msg.pos.y,self.people_msg.pos.z),self.q)) self.move_base.send_goal(goal) rospy.spin()
def run(self): global FLAG,PEOPLE_POSITION lock = threading.Lock() # while True: if FLAG!=0: with lock: rospy.loginfo(PEOPLE_POSITION) x=PEOPLE_POSITION.x-0.7 y=PEOPLE_POSITION.y self.goal.target_pose.pose=(Pose(Point(x,y,0),self.q)) subprocess.call(["rosservice","call","/move_base/clear_costmaps"]) self.move_base.send_goal(self.goal) FLAG=0 rospy.sleep(0.1) return TaskStatus.RUNNING
def __init__(self): rospy.init_node('openhmd_ros') self.pub = rospy.Publisher('/openhmd/pose', Pose) self.hmd = None
def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): d = self.hmd.poll() if len(d) != 4: continue pose = Pose() pose.orientation = Quaternion(d[0], d[1], d[2], d[3]) euler = euler_from_quaternion(d) pose.position = Point(euler[0], euler[1], euler[2]) self.pub.publish(pose) r.sleep()
def getPose(self): p=self.endpoint_pose() if len(p.keys())==0: rospy.logerr("ERROR: Pose is empty, you may want to wait a bit before calling getPose to populate data") return None pose=Pose() pose.position=Point(*p["position"]) pose.orientation=Quaternion(*p["orientation"]) return pose
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 particle_to_pose(particle): pose = Pose() pose.position.x = particle[0] pose.position.y = particle[1] pose.orientation = angle_to_quaternion(particle[2]) return pose
def __init__(self, id, position, quaternion): self.id = id self.pose = Pose(position, quaternion);
def create_pose(location, rotation): if len(location) >= 3: x, y, z = location[0:3] else: x = y = z = 0 if len(rotation) >= 4: qw, qx, qy, qz = rotation[0:4] else: qw = 1 qx = qy = qz = 0 return geom_msgs.Pose( position=geom_msgs.Point(x=x, y=y, z=z), orientation=geom_msgs.Quaternion(w=qw, x=qx, y=qy, z=qz) )
def set_pose_targets(self, poses, end_effector_link = ""): """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, 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(): if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link): raise MoveItCommanderException("Unable to set target poses") else: raise MoveItCommanderException("There is no end effector to set poses for")
def get_object_poses(self, object_ids): """ Get the poses from the objects identified by the given object ids list. """ ser_ops = self._psi.get_object_poses(object_ids) ops = dict() for key in ser_ops: msg = Pose() conversions.msg_from_string(msg, ser_ops[key]) ops[key] = msg return ops
def to_message(self): """ Return a nav_msgs/OccupancyGrid representation of this map. """ grid_msg = OccupancyGrid() # Set up the header. grid_msg.header.stamp = rospy.Time.now() grid_msg.header.frame_id = "map" # .info is a nav_msgs/MapMetaData message. grid_msg.info.resolution = self.resolution grid_msg.info.width = self.width grid_msg.info.height = self.height # Rotated maps are not supported... quaternion represents no # rotation. grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0.), Quaternion(0., 0., 0., 1.)) # Flatten the numpy array into a list of integers from 0-100. # This assumes that the grid entries are probalities in the # range 0-1. This code will need to be modified if the grid # entries are given a different interpretation (like # log-odds). flat_grid = self.grid.reshape((self.grid.size,)) * 100. grid_msg.data = list(np.round(flat_grid, decimals = 3)) return grid_msg
def dict_to_msg(pose_dict): pose_msg = Pose() pose_msg.position = pose_dict['position'] pose_msg.orientation = pose_dict['orientation'] return pose_msg
def dragEnterEvent(self, event): if event.mimeData().hasText(): topic_name = str(event.mimeData().text()) if len(topic_name) == 0: qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty') return else: if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0: qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0') return item = event.source().selectedItems()[0] topic_name = item.data(0, Qt.UserRole) if topic_name is None: qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name') return # check for valid topic msg_class, self._topic_name, _ = get_topic_class(topic_name) if msg_class is None: qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name) return # check for valid message class quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class) if quaternion_slot_path is None and point_slot_path is None: qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".' % (msg_class._type, topic_name)) return event.acceptProposedAction()
def _get_slot_paths(msg_class): # find first Pose in msg_class pose_slot_paths = find_slots_by_type_bfs(msg_class, Pose) for path in pose_slot_paths: # make sure the path does not contain an array, because we don't want to deal with empty arrays... if '[' not in path: path = PoseViewWidget._make_path_list_from_path_string(pose_slot_paths[0]) return path + ['orientation'], path + ['position'] # if no Pose is found, find first Quaternion and Point quaternion_slot_paths = find_slots_by_type_bfs(msg_class, Quaternion) for path in quaternion_slot_paths: if '[' not in path: quaternion_slot_path = PoseViewWidget._make_path_list_from_path_string(path) break else: quaternion_slot_path = None point_slot_paths = find_slots_by_type_bfs(msg_class, Point) for path in point_slot_paths: if '[' not in path: point_slot_path = PoseViewWidget._make_path_list_from_path_string(path) break else: point_slot_path = None return quaternion_slot_path, point_slot_path
