def get_odom(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), quat_to_angle(Quaternion(*rot)))
python类Point()的实例源码
def get_position(self):
# Get the current transform between the odom and base frames
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return Point(*trans)
def publishPosition(self):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/target_' + str(self.tag)
msg.point = Point(self.estimated_position[0], self.estimated_position[1], 0)
self.position_publisher.publish(msg)
self.publishCovarianceMatrix()
def publishPosition(self):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.point = Point(self.position[0], self.position[1], 0)
self.publisher.publish(msg)
def publishPosition(self):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.point = Point(self.position[0], self.position[1], 0)
self.publisher.publish(msg)
def publishPosition(self):
msg = PointStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
msg.point = Point(self.position[0], self.position[1], 0)
self.publisher.publish(msg)
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 jacobian(self, group_name, joint_state, use_quaternion=False, link=None, ref_point=None):
def compute_jacobian_srv():
rospy.wait_for_service('compute_jacobian')
try:
compute_jac = rospy.ServiceProxy('compute_jacobian', GetJacobian)
js = JointState()
js.name = self.get_joint_names(group_name)
js.position = self.extract_group_joints(group_name, joint_state)
p = Point(x=ref_point[0], y=ref_point[1], z=ref_point[2])
# call the service
res = compute_jac(group_name, link, js, p, use_quaternion)
# reorganize the jacobian
jac_array = np.array(res.jacobian).reshape((res.nb_rows, res.nb_cols))
# reorder the jacobian wrt to the joint state
ordered_jac = np.zeros((len(jac_array), len(joint_state.name)))
for i, n in enumerate(js.name):
ordered_jac[:, joint_state.name.index(n)] = jac_array[:, i]
return ordered_jac
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# compute the jacobian only for chains
# if group_name not in ['right_arm', 'left_arm', 'head', 'right_leg', 'left_leg']:
# rospy.logerr('The Jacobian matrix can only be computed on kinematic chains')
# return []
# assign values
if link is None:
link = self.end_effectors[group_name]
if ref_point is None:
ref_point = [0, 0, 0]
# return the jacobian
return compute_jacobian_srv()
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 get_angle(self, link, spin_center, steer_angle, stamp):
# lookup the position of each link in the back axle frame
ts = self.tf_buffer.lookup_transform(spin_center.header.frame_id, link,
stamp, rospy.Duration(4.0))
dy = ts.transform.translation.y - spin_center.point.y
dx = ts.transform.translation.x - spin_center.point.x
angle = math.atan2(dx, abs(dy))
if steer_angle < 0:
angle = -angle
# visualize the trajectory forward or back of the current wheel
# given the spin center
radius = math.sqrt(dx * dx + dy * dy)
self.marker.points = []
for pt_angle in numpy.arange(abs(angle) - 1.0, abs(angle) + 1.0 + 0.025, 0.05):
pt = Point()
pt.x = spin_center.point.x + radius * math.sin(pt_angle)
if steer_angle < 0:
pt.y = spin_center.point.y - radius * math.cos(pt_angle)
else:
pt.y = spin_center.point.y + radius * math.cos(pt_angle)
self.marker.ns = link
self.marker.header.stamp = stamp
self.marker.points.append(pt)
self.marker_pub.publish(self.marker)
return angle, radius
# TODO(lucasw) want to receive a joint state that has position
# and velocity and command a joint_state to achieve it, but ros_control
# can only take a command for a single value, would need a pvt style
# interface running upstream of ros_control, or make custom own ros_control
# controller?
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 __init__(self):
self.pt = None
self.pts = []
self.image = None
self.bridge = CvBridge()
self.sub_pt = rospy.Subscriber(\
"/zed/rgb/image_rect_color_mouse_rgb", Point, self.cb_pt)
self.sub_image = rospy.Subscriber(\
"/zed/rgb/image_rect_color", Image, self.cb_img)
def add(self, marker):
for i in range(len(self.keyPointList)):
if (self.keyPointList[i].id==marker.id):
return
position = self.transformMarkerToWorld(marker)
k = KeyPoint(marker.id, Point(position[0], position[1], position[2]), Quaternion(0., 0., 0., 1.))
self.keyPointList.append(k)
self.addWaypointMarker(k)
rospy.loginfo('Marker is added to following position')
rospy.loginfo(k.pose)
pass
def addWaypointMarker(self, keyPoint):
rospy.loginfo('Publishing marker')
# Set up our waypoint markers
marker_scale = 0.2
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = keyPoint.id
marker_color = {'r': 0.0, 'g': 1.0, 'b': 0.0, 'a': 1.0}
# Initialize the marker points list.
self.waypoint_markers = Marker()
self.waypoint_markers.ns = marker_ns
self.waypoint_markers.id = marker_id
self.waypoint_markers.type = Marker.CUBE_LIST
self.waypoint_markers.action = Marker.ADD
self.waypoint_markers.lifetime = rospy.Duration(marker_lifetime)
self.waypoint_markers.scale.x = marker_scale
self.waypoint_markers.scale.y = marker_scale
self.waypoint_markers.color.r = marker_color['r']
self.waypoint_markers.color.g = marker_color['g']
self.waypoint_markers.color.b = marker_color['b']
self.waypoint_markers.color.a = marker_color['a']
self.waypoint_markers.header.frame_id = 'map'
self.waypoint_markers.header.stamp = rospy.Time.now()
self.waypoint_markers.points = list()
p = Point(keyPoint.pose.position.x, keyPoint.pose.position.y, keyPoint.pose.position.z)
self.waypoint_markers.points.append(p)
# Publish the waypoint markers
self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
self.marker_pub.publish(self.waypoint_markers)
def main():
set_location_service = rospy.ServiceProxy('set_camera_location', services.SetCameraLocation)
set_rotation_service = rospy.ServiceProxy('set_camera_rotation', services.SetCameraRotation)
get_image_service = rospy.ServiceProxy('get_camera_view', services.GetCameraImage)
set_location_service(0, geom_msgs.Point(x=100, y=-270, z=100))
set_rotation_service(0, geom_msgs.Quaternion(w=0.70710678, x=0, y=0, z=-0.70710678))
response = get_image_service(0, 'depth')
opencv_bridge = cv_bridge.CvBridge()
image = opencv_bridge.imgmsg_to_cv2(response.image)
cv2.imshow('test', image)
cv2.waitKey()
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 __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face.indices[0],
face.indices[1],
face.indices[2]]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def to3dPoints(points_2d):
"""Simple function to take list of 2d coordinates and output list of 3d ros points"""
points_3d = []
for (x,y) in points_2d:
p = Point()
p.x = x
p.y = y
p.z = 0
points_3d.append(p)
return points_3d
