def copDetection(): copName = "deckard" robberName = "roy" # Get list of objects and their locations mapInfo = 'map2.yaml' objLocations = getObjects(mapInfo) vertexes = objLocations.values() vertexKeys = objLocations.keys() # TODO: GetPlan not working because move base does not provide the service (EITHER FIX IT OR FIND A NEW WAY TO GET DISTANCE) # tol = .1 # robberName = "roy" # robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,0,1))) # destination = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1))) # print(robLoc) # print(destination) # # Get plan from make_plan service # #rospy.wait_for_service("/" + robberName + "move_base/make_plan") # try: # planner = rospy.ServiceProxy("/" + robberName + "/move_base/make_plan", nav_srv.GetPlan) # plan = planner(robLoc, destination, tol) # poses = plan.plan.poses # print(poses) # except rospy.ServiceException, e: # print "GetPlan service call failed: %s"%e # return 0 # rospy.Subscriber("/" + copName + "/", geo_msgs.Pose, getCopLocation) # rospy.Subscriber("/" + robberName + "/", geo_msgs.Pose, getRobberLocation) copLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,1,0))) pastCopLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,1,0))) robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,1,0))) test_path = [geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1))), geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,0,1)))] test_plans = {"kitchen": nav_msgs.Path(std_msgs.Header(), test_path)} curCost, curDestination = chooseDestination(test_plans, copLoc, robLoc, pastCopLoc) print("Minimum Cost: " + str(curCost) + " at " + curDestination) #move base to curDestination # state = mover_base.get_state() # pathFailure = False # while (state!=3 and pathFailure==False): # SUCCESSFUL # #wait a second rospy.sleep(1) # newCost = evaluatePath(test_plans[curDestination]) # if newCost > curCost*2: # pathFailure = True # rospy.loginfo("Just Reached " + vertexKeys[i]) # Floyd warshall stuff mapGrid = np.load('mapGrid.npy') floydWarshallCosts = np.load('floydWarshallCosts.npy') evaluateFloydCost(copLoc, robLoc, floydWarshallCosts, mapGrid)
def publishTagMarkers(tags, publisher): """Republishes the tag_markers topic""" SHIFT = 0.05 h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map') ca = std_msgs.ColorRGBA(r=1, a=1) cb = std_msgs.ColorRGBA(g=1, a=1) ct = std_msgs.ColorRGBA(b=1, a=1) sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5) sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25) st = geometry_msgs.Vector3(x=1, y=1, z=1) ma = visualization_msgs.MarkerArray() ma.markers.append( visualization_msgs.Marker( header=h, id=-1, action=visualization_msgs.Marker.DELETEALL)) for i, t in enumerate(tags): th = t['th_deg'] * math.pi / 180. q = tf.transformations.quaternion_from_euler(0, 0, th) q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t['x'], y=t['y']), orientation=q), scale=sa, color=ca)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 1, type=visualization_msgs.Marker.CUBE, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point( x=t['x'] + SHIFT * math.cos(th), y=t['y'] + SHIFT * math.sin(th)), orientation=q), scale=sb, color=cb)) ma.markers.append( visualization_msgs.Marker( header=h, id=i * 3 + 2, type=visualization_msgs.Marker.TEXT_VIEW_FACING, action=visualization_msgs.Marker.ADD, pose=geometry_msgs.Pose( position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5), orientation=q), scale=st, color=ct, text=str(t['id']))) publisher.publish(ma)
def get_up_stretched_0123wa(): fl_pose = geomsg.Pose() fl_pose.position.x = 0.35 fl_pose.position.y = 0.35 fl_pose.position.z = -0.7 fl_pose.orientation.x = 0.0 fl_pose.orientation.y = 0.0 fl_pose.orientation.z = 0.0 fl_pose.orientation.w = 1.0 fr_pose = geomsg.Pose() fr_pose.position.x = 0.35 fr_pose.position.y = -0.35 fr_pose.position.z = -0.7 fr_pose.orientation.x = 0.0 fr_pose.orientation.y = 0.0 fr_pose.orientation.z = 0.0 fr_pose.orientation.w = 1.0 rr_pose = geomsg.Pose() rr_pose.position.x = -0.35 rr_pose.position.y = -0.35 rr_pose.position.z = -0.7 rr_pose.orientation.x = 0.0 rr_pose.orientation.y = 0.0 rr_pose.orientation.z = 0.0 rr_pose.orientation.w = 1.0 rl_pose = geomsg.Pose() rl_pose.position.x = -0.35 rl_pose.position.y = 0.35 rl_pose.position.z = -0.7 rl_pose.orientation.x = 0.0 rl_pose.orientation.y = 0.0 rl_pose.orientation.z = 0.0 rl_pose.orientation.w = 1.0 waist = geomsg.Pose() waist.position.x = 0.0 waist.position.y = 0.0 waist.position.z = 0.95 waist.orientation.x = 0.0 waist.orientation.y = 0.0 waist.orientation.z = 0.0 waist.orientation.w = 1.0 return (fl_pose, fr_pose, rr_pose, rl_pose, waist)
def moveto_action(self, position, orientation): self.client.cancel_goal() rospy.logwarn("Going to waypoint") rospy.logwarn("Found server") # Stay under the water if position[2] > 0.3: rospy.logwarn("Waypoint set too high!") position[2] = -0.5 goal = mil_msgs.MoveToGoal( header=mil_ros_tools.make_header('/map'), posetwist=mil_msgs.PoseTwist( pose=geom_msgs.Pose( position=mil_ros_tools.numpy_to_point(position), orientation=mil_ros_tools.numpy_to_quaternion(orientation) ) ), speed=0.2, linear_tolerance=0.1, angular_tolerance=0.1 ) self.client.send_goal(goal) # self.client.wait_for_result() rospy.logwarn("Go to waypoint")
def execute(self, ud): start_time = rospy.get_time() rate = rospy.Rate(2.5) while not self.preempt_requested() and not rospy.is_shutdown(): segmented_objs = self.segment_srv( only_surface=True).segmented_objects.objects if segmented_objs: table = segmented_objs[0] pose = geo_msgs.PoseStamped( table.point_cloud.header, geo_msgs.Pose(table.center, table.orientation)) pose = TF2().transform_pose(pose, pose.header.frame_id, 'map') width, length = table.width, table.depth table_tf = to_transform(pose, 'table_frame') TF2().publish_transform(table_tf) if 'table' in ud: table.name = ud['table'].name ud['table'] = table ud['table_pose'] = pose rospy.loginfo("Detected table of size %.1f x %.1f at %s", width, length, pose2d2str(pose)) return 'succeeded' elif self.timeout and rospy.get_time() - start_time > self.timeout: rospy.logwarn("No table detected after %g seconds", rospy.get_time() - start_time) return 'aborted' try: rate.sleep() except rospy.ROSInterruptException: break rospy.logwarn( "Detect table has been preempted after %g seconds (timeout %g)", rospy.get_time() - start_time, self.timeout) return 'preempted'
def start_MM(self, mess): if mess.data == True: print("MissionManagment Started") #Intial Pose camera_initial_pose = geo_msg.Pose() camera_initial_pose.position.x = 0.2 camera_initial_pose.position.y = -0.4 camera_initial_pose.position.z = 0.4 camera_initial_pose.orientation.x = 0 camera_initial_pose.orientation.y = 1 camera_initial_pose.orientation.z = 0 camera_initial_pose.orientation.w = 0 self.pose_ur_pub.publish(camera_initial_pose) time.sleep(5) startEMVS = self.startEMVScall(True) time.sleep(1) #Velocity Command camera_vel_cmd = geo_msg.Twist() camera_vel_cmd.linear.x = 0.03 camera_vel_cmd.linear.y = -0.006 camera_vel_cmd.linear.z = 0 camera_vel_cmd.angular.x = 0 camera_vel_cmd.angular.y = 0 camera_vel_cmd.angular.z = 0 self.vel_ur_pub.publish(camera_vel_cmd)
def test_pose_init(self): """ Test if pose class can be initialized as expected. """ # Create from quaternion p = Point(1, 3, 2) o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2)) pose = Pose(p, o) # Create from angle pose2 = Pose(p, math.pi / 2) self.assertEqual(pose, pose2) g_pose = g_msgs.Pose() g_pose.position = p.to_geometry_msg() g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2), math.sqrt(1 / 2)) self.assertEqual(Pose(g_pose), pose) # Create from geometry_msgs/transform g_tf = g_msgs.Transform() g_tf.translation = p.to_geometry_msg() g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2), math.sqrt(1 / 2)) self.assertEqual(Pose(g_tf), pose)
def vel_callback(msg): global pose_lock, saved_pose twist = gms.Twist() twist.linear.x = msg.linear[0] twist.linear.y = msg.linear[1] twist.linear.z = msg.linear[2] twist.angular.x = msg.angular[0] twist.angular.y = msg.angular[1] twist.angular.z = msg.angular[2] twist_pub.publish(twist) pose_lock.acquire() ps = cp.copy(saved_pose) pose_lock.release() if not ps is None: translation = ps.position rotation = ps.orientation cmd_pose = gms.PoseStamped() cmd_pose.pose = gms.Pose( position=translation, orientation=rotation) cmd_pose_pub.publish(cmd_pose) transform = gms.Transform(translation, rotation) cmd_traj = tms.MultiDOFJointTrajectory() cmd_traj.points.append(tms.MultiDOFJointTrajectoryPoint()) cmd_traj.points[0].transforms.append(transform) cmd_traj.points[0].velocities.append(twist) cmd_traj_pub.publish(cmd_traj)
def test_sd_pose(self): """ Initial pose of franka panda_arm group position: x: 0.307019570052 y: -5.22132961561e-12 z: 0.590269558277 orientation: x: 0.923955699469 y: -0.382499497279 z: 1.3249325839e-12 w: 3.20041176635e-12 :return: """ ros_pose = GeometryMsg.Pose() ros_pose.position.x = 0.307019570052 ros_pose.position.y = 0 ros_pose.position.z = 0.590269558277 ros_pose.orientation.x = 0.923955699469 ros_pose.orientation.y = -0.382499497279 ros_pose.orientation.z = 0 ros_pose.orientation.w = 0 pose_mat = common.sd_pose(ros_pose) re_pose = common.to_ros_pose(pose_mat) self.assertTrue(common.all_close(ros_pose, re_pose, 0.001))
def getObjects(mapInfo): with open(mapInfo, 'r') as stream: try: yamled = yaml.load(stream) except yaml.YAMLError as exc: print(exc) # deletes info del yamled['info'] # Populates dictionary with location names and their poses objDict = yamled.values() objLocations = {} objNames = {} for item in objDict: itemName = item['name'] if itemName[0:4] != "wall": x_loc = item['centroid_x'] + (item['width'] / 2 + .6) * math.cos( math.radians(item['orientation'])) y_loc = item['centroid_y'] + (item['length'] / 2 + .6) * math.sin( math.radians(item['orientation'])) quat = tf.transformations.quaternion_from_euler( 0, 0, item['orientation'] - 180) itemLoc = geo_msgs.PoseStamped( std_msgs.Header(), geo_msgs.Pose( geo_msgs.Point(x_loc, y_loc, 0), geo_msgs.Quaternion(quat[0], quat[1], quat[2], quat[3]))) objLocations[itemName] = itemLoc objNames[itemName] = ([item['value']]) return objLocations, objNames
def build_cart_request(pos, quat, arm, initial_state = INITIAL_ROBOT_STATE, planner_id=''): m = MotionPlanRequest() m.group_name = "%s_arm" % arm target_link = "%s_wrist_roll_link" % arm[0] m.start_state = initial_state m.planner_id = planner_id pc = PositionConstraint() pc.link_name = target_link pc.header.frame_id = 'odom_combined' pose = gm.Pose() pose.position = pos pc.constraint_region.primitive_poses = [pose] sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [.01] pc.constraint_region.primitives = [ sphere ] oc = OrientationConstraint() oc.link_name = target_link oc.header.frame_id = 'odom_combined' oc.orientation = quat c = Constraints() c.position_constraints = [ pc ] c.orientation_constraints = [ oc ] m.goal_constraints = [ c ] m.allowed_planning_time = rospy.Duration(args.max_planning_time) return m
def make_pose_stamped(position, orientation, frame='/body'): wrench = geometry_msgs.WrenchStamped( header=make_header(frame), pose=geometry_msgs.Pose( force=geometry_msgs.Vector3(*position), orientation=geometry_msgs.Quaternion(*orientation))) return wrench
def pub_loc(self,publisher): pub = geo.Pose() if len(self.nums)<2: pub.position.x = -10 pub.position.y = -10 pub.position.z = -10 elif len(self.nums)>=5: pub.position.x = 10 pub.position.y = 10 pub.position.z = 10 self.done = True else: nxt = self.next_loc() pub.position.x = nxt[0] pub.position.y = nxt[1] if pub.position.x > 3.5: pub.position.x = 3.5 elif pub.position.x < 0.0: pub.position.x = 0.0 if pub.position.y > 3.3: pub.position.y = 3.3 elif pub.position.y < -2.0: pub.position.y = -2.0 eul = (0.0,0.0,math.atan2(nxt[1]-1, nxt[0]-2)) quat = tft.quaternion_from_euler(0.0,0.0, math.atan2(nxt[1],nxt[0])) pub.orientation.x = quat[0] pub.orientation.y = quat[1] pub.orientation.z = quat[2] pub.orientation.w = quat[3] publisher.publish(pub)
def PoseStamped(x=0, y=0, z=0, phi=0, roll=0, pitch=0, yaw=0, frame_id="/map", stamp=None, pointstamped=None): """Build a geometry_msgs.msgs.PoseStamped from any number of arguments. Each value defaults to 0 >>> ps = PoseStamped(yaw=0.5) >>> ps.pose position: x: 0 y: 0 z: 0 orientation: x: 0.0 y: 0.0 z: 0.247403959255 w: 0.968912421711 """ if not stamp: stamp = rospy.get_rostime() if pointstamped: return gm.PoseStamped(header=pointstamped.header, pose=gm.Pose(pointstamped.point, Quaternion())) elif isinstance(x, number) and isinstance(y, number) and isinstance(z, number): return gm.PoseStamped(header=Header(frame_id, stamp), pose=Pose(x, y, z, phi, roll, pitch,yaw)) else: raise ValueError("Either supply a number for x, y and z or a PointStamped to pointstamped")
def execute(self, userdata): rospy.loginfo('Executing state CLEAR WORKSPACE') # CHANGE CONTROLLER TO WHEELED MOTION robot.load_controller('WheeledMotion') # CALL ACTION TO GO DOWN AND WAIT FOR RESULT (l_pose, r_pose) = poses.get_clear_workspace_lr() waist_delta_pose = geomsg.Pose() waist_delta_pose.position.z = -0.1 robot.go_to('waist', waist_delta_pose, 6.0, True) robot.go_to('fl_wheel', l_pose, 6.0) robot.go_to('fr_wheel', r_pose, 6.0) robot.wait_for_result('waist') robot.wait_for_result('fl_wheel') robot.wait_for_result('fr_wheel') # BACK TO FIXED-WHEELS CONTROLLER robot.load_controller('OpenSot') text = '' while not (text == 'Y' or text == 'n'): text = raw_input('Workspace cleared correcly? [Y/n]') if text == 'Y': return 'ws_cleared' else: return 'failure'
def publish_pose(self): '''TODO: Publish velocity in body frame ''' linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0)) # TODO: Not 100% on this transpose angular_vel = self.orientation.transpose().dot(self.angular_vel) quaternion = self.body.getQuaternion() translation = self.body.getPosition() header = sub8_utils.make_header(frame='/world') pose = geometry.Pose( position=geometry.Point(*translation), orientation=geometry.Quaternion(-quaternion[1], -quaternion[2], -quaternion[3], quaternion[0]), ) twist = geometry.Twist( linear=geometry.Vector3(*linear_vel), angular=geometry.Vector3(*angular_vel) ) odom_msg = Odometry( header=header, child_frame_id='/body', pose=geometry.PoseWithCovariance( pose=pose ), twist=geometry.TwistWithCovariance( twist=twist ) ) self.truth_odom_pub.publish(odom_msg)
def pub_target_timer_callback(self): (xyz, rpy) = planner_instance._target_pose q = tf.transformations.quaternion_from_euler(*rpy) msg = geometry_msgs.Pose(geometry_msgs.Point(*xyz), geometry_msgs.Quaternion(*q)) publisher.publish(msg)
def distance_2d(pose1, pose2=None): if not pose2: pose2 = pose1 pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin return sqrt( pow(pose2.position.x - pose1.position.x, 2) + pow(pose2.position.y - pose1.position.y, 2))
def offset_ros_pose(pose, offset): output = GeometryMsg.Pose() output.position.x = pose.position.x + offset[0] output.position.y = pose.position.y + offset[1] output.position.z = pose.position.z + offset[2] output.orientation = pose.orientation return output
def Pose(x=0, y=0, z=0, phi=0, roll=0, pitch=0, yaw=0): """ >>> pose = Pose(yaw=0.5) >>> pose position: x: 0 y: 0 z: 0 orientation: x: 0.0 y: 0.0 z: 0.247403959255 w: 0.968912421711 """ if phi: rospy.logerr( "Please specify yaw instead of phi. Phi will be removed!!!") z_rotation = phi or yaw #Get the one that is not 0 quat = Quaternion(roll=roll, pitch=pitch, yaw=z_rotation) pos = Point(x, y, z) return gm.Pose(pos, quat)
def __init__(self, pose_topic='/pick_up_parcel/pose', twist_topic='/pick_up_parcel/cmd_vel', duration=None): super(PickUpParcel, self).__init__(action_name="pick_up_parcel", action_type=dyno_msgs.QuadrotorPickUpParcelAction, worker=self.worker, duration=duration) self.initial_height = 1.0 self.goal_height = 0.2 self.pose = geometry_msgs.Pose() self.pose.position.z = self.initial_height self.twist = geometry_msgs.Twist() latched = True queue_size_five = 1 self.publishers = py_trees_ros.utilities.Publishers([ ('pose', pose_topic, geometry_msgs.Pose, latched, queue_size_five), ('twist', twist_topic, geometry_msgs.Twist, latched, queue_size_five) ]) # self.publishers.pose.publish(self.pose) # self.publishers.twist.publish(self.twist) self.publishing_timer = rospy.Timer(period=rospy.Duration(0.5), callback=self.publish, oneshot=False) self.start()
def heading(pose1, pose2=None): """ Heading angle from one pose to another. Poses are assumed to have the same reference frame. """ if not pose2: pose2 = pose1 pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin p1, p2 = __get_naked_poses(pose1, pose2) return atan2(p2.position.y - p1.position.y, p2.position.x - p1.position.x)
def move_to_pos_quat(group, pos, quat): pose_goal = gm.Pose(gm.Point(*pos), gm.Quaternion(*quat)) group.set_pose_target(pose_goal) plan_success = group.go(wait=True) group.stop() group.clear_pose_targets() return plan_success
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 set_random_pose(self, center, scale): '''Set a random position, and for now constant orientation TODO: Random orientation ''' pos = center + (np.random.random(3) * scale) self.set_pose(pose=geometry_msgs.Pose( position=pos, orientation=(0.0, 0.0, 0.0, 1.0), ))
def generate_plan(): target_pose = geometry_msg_lib.Pose() target_pose.orientation.w = 1.0 target_pose.position.x = 0.7 target_pose.position.y = -0.05 target_pose.position.z = 1.1 move_group.set_pose_target(target_pose) return move_group.plan()
def draw_curve(arr): pose_array = gmm.PoseArray() for (x, y, z) in arr: pose = gmm.Pose() pose.position = gmm.Point(x, y, z) pose_array.poses.append(pose) pose_array.header.frame_id = "/base_footprint" pose_array.header.stamp = rospy.Time.now() rviz.draw_curve(pose_array)
def distance_2d(pose1, pose2=None): """ Euclidean distance between 2D poses; z coordinate is ignored. Poses are assumed to have the same reference frame. """ if not pose2: pose2 = pose1 pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin p1, p2 = __get_naked_poses(pose1, pose2) return sqrt( pow(p2.position.x - p1.position.x, 2) + pow(p2.position.y - p1.position.y, 2))
def to_pose3d(pose, timestamp=rospy.Time(), frame=None): if isinstance(pose, geometry_msgs.Pose2D): p = geometry_msgs.Pose(geometry_msgs.Point(pose.x, pose.y, 0.0), quaternion_msg_from_yaw(pose.theta)) if not frame: return p return geometry_msgs.PoseStamped(std_msgs.Header(0, timestamp, frame), p) raise rospy.ROSException( "Input parameter pose is not a geometry_msgs.Pose2D object")
def tup_to_pose(p): pose = gmsg.Pose() pose.position.x = p[0][0] pose.position.y = p[0][1] pose.position.z = p[0][2] pose.orientation.x = p[1][0] pose.orientation.y = p[1][1] pose.orientation.z = p[1][2] pose.orientation.w = p[1][3] return pose