def make_pose_covariance_stamped_msg(t,R,Cov): """ Returns a pose stamped message from a translation vector and rotation matrix (4x4) for publishing. NOTE: Does not set the target frame. """ pose_cov_stamped_msg = PoseWithCovarianceStamped() # pose_cov_stamped_msg.header = Header() pose_cov_stamped_msg.header.stamp = rospy.Time.now() # pose_msg = Pose() pose_msg.position.x = t[0] pose_msg.position.y = t[1] pose_msg.position.z = t[2] # quat = quaternion_from_matrix(R) pose_msg.orientation.x = quat[0] pose_msg.orientation.y = quat[1] pose_msg.orientation.z = quat[2] pose_msg.orientation.w = quat[3] # pose_cov_stamped_msg.pose.pose = pose_msg pose_cov_stamped_msg.pose.covariance = Cov return pose_cov_stamped_msg
def generate_pose(self, angle): # Generate a pose, all values but the yaw will be 0. q = transformations.quaternion_from_euler(0, 0, angle) header = Header( stamp=rospy.Time.now(), frame_id="map" ) pose = Pose( position=Point(x=0, y=0, z=0), orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) ) # Publish pose stamped - just for displaying in rviz self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() covariance = np.array([1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, .7])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.pose = p_c # Publish pose estimation self.p_c_s_est_pub.publish(p_c_s)
def execute(self, userdata): p = PoseWithCovarianceStamped() p.pose = userdata.pose_current rospy.loginfo(userdata.pose_current) self.initialpose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped) self.initialpose_pub.publish(p) return 'succeeded'
def state2pose(self, state, cov): #print(cov) output = PoseWithCovarianceStamped() output.pose.pose.position.x = state[0] output.pose.pose.position.y = state[1] output.pose.pose.position.z = 0 oppo = tf.transformations.quaternion_from_euler(0,0,state[2]) output.pose.pose.orientation.x = oppo[0] output.pose.pose.orientation.y = oppo[1] output.pose.pose.orientation.z = oppo[2] output.pose.pose.orientation.w = oppo[3] new_cov = self.robot_whole_cov #print(new_cov) new_cov[0] = cov[0] new_cov[1] = cov[1] new_cov[5] = cov[2] new_cov[6] = cov[3] new_cov[7] = cov[4] new_cov[11] = cov[5] new_cov[30] = cov[6] new_cov[31] = cov[7] new_cov[35] = cov[8] output.pose.covariance = new_cov.flatten() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'base_link' output.header = h return output
def get_data_body(self, body_id): try: resp1 = self.call_body_data(body_id) except rospy.ServiceException as exc: print("Service did not process request: " + str(exc)) msg = PoseWithCovarianceStamped() pose = Pose() pose.position.x = resp1.pos.x pose.position.y = resp1.pos.y pose.position.z = resp1.pos.z quaternion = tf.transformations.quaternion_from_euler(resp1.pos.roll, resp1.pos.pitch, resp1.pos.yaw) #type(pose) = geometry_msgs.msg.Pose pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] h = std_msgs.msg.Header() h.stamp = rospy.Time.now() msg.header = h msg.header.frame_id= 'world' msg.pose.covariance = self.mocap_covariance msg.pose.pose = pose return msg
def default(self, ci='unused'): if 'valid' not in self.data or self.data['valid']: pose = PoseWithCovarianceStamped() pose.header = self.get_ros_header() pose.pose.pose = get_pose(self) if 'covariance_matrix' in self.data: pose.pose.covariance = self.data['covariance_matrix'] self.publish(pose)
def publishInitialPose(xPos, yPos, angle): pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped) start = PoseWithCovarianceStamped() start.header = g.header start.pose.pose.position.x = xPos start.pose.pose.position.y = yPos start.pose.pose.orientation = g.quat pub.publish(start)
def default(self, ci="unused"): if "valid" not in self.data or self.data["valid"]: pose = PoseWithCovarianceStamped() pose.header = self.get_ros_header() pose.pose.pose = get_pose(self) if "covariance_matrix" in self.data: pose.pose.covariance = self.data["covariance_matrix"] self.publish(pose)
def _init_nav(self, pose): pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) rospy.sleep(1.0) initialPose = PoseWithCovarianceStamped() initialPose.header = pose.header initialPose.pose.pose = pose.pose p_cov = np.array([0.0]*36) initialPose.pose.covariance = tuple(p_cov.ravel().tolist()) pub.publish(initialPose)
def reset_pose(self): """Sets the current pose to START. Doesn't move the robot.""" loginfo("Resetting pose.") req = PoseWithCovarianceStamped() req.header = Header(stamp=Time.now(), frame_id='/map') req.pose.pose = self._x_y_yaw_to_pose(START_X, START_Y, START_YAW) req.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.initial_pose_pub.publish(req) self.go_to_start()
def publish_pose(self,pose,time): #Generate pose q = tf.transformations.quaternion_from_euler(0, 0, pose[2]) header = Header( stamp=rospy.Time.now(), frame_id="map" ) pose = Pose( position=Point( x=pose[0], y=pose[1], z=0 ), orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3], ) ) # Publish pose stamped self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() # These don't matter covariance = np.array([ 1, 0, 0, 0, 0, 0, 0,.2*self.std_err, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, self.std_err])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.header.frame_id = "map" p_c_s.pose = p_c # if time.time() - self.start_time < 5: # self.p_c_s_init_pub.publish(p_c_s) # else: self.p_c_s_est_pub.publish(p_c_s)
def state2pose(self, state, cov): output = PoseWithCovarianceStamped() output.pose.pose.position.x = state[0] output.pose.pose.position.y = state[1] cov_zeros = np.zeros((36,)) cov_zeros[0] = cov[0,0] cov_zeros[1] = cov[0,1] cov_zeros[6] = cov[1,0] cov_zeros[7] = cov[1,1] output.pose.covariance = cov_zeros.flatten() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'odom' output.header = h return output
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 _push_mocap_pose(self,data): if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"): t = self.tf.getLatestCommonTime(self.frame, "/world") position, quaternion = self.tf.lookupTransform("/world", self.frame, t) msg = PoseWithCovarianceStamped() pose = PoseWithCovariance() pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] pose.pose.orientation.x = quaternion[0] pose.pose.orientation.y = quaternion[1] pose.pose.orientation.z = quaternion[2] pose.pose.orientation.w = quaternion[3] pose.covariance = [0] * 36 #could tune the covariance matrices? pose.covariance[0] = -1 msg.pose = pose msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/bug/base_link" self.pose_converted.publish(msg)
def state2pose_seq(self, state, cov): robot_output = PoseWithCovarianceStamped() robot_output.pose.pose.position.x = state[0] robot_output.pose.pose.position.y = state[1] robot_cov = np.zeros((36,)) robot_cov[0] = cov[0,0] robot_cov[1] = cov[0,1] robot_cov[6] = cov[1,0] robot_cov[7] = cov[1,1] robot_output.pose.covariance = robot_cov.flatten() oppo = tf.transformations.quaternion_from_euler(0,0,self.yaw) robot_output.pose.pose.orientation.x = oppo[0] robot_output.pose.pose.orientation.y = oppo[1] robot_output.pose.pose.orientation.z = oppo[2] robot_output.pose.pose.orientation.w = oppo[3] h = std_msgs.msg.Header() h.stamp = self.t #rospy.Time.now() h.frame_id = 'odom' robot_output.header = h return robot_output
def publish_pose(self,pose,stamp): #Generate pose q = tf.transformations.quaternion_from_euler(0, 0, pose[2]) header = Header( stamp=stamp, frame_id="map" ) pose = Pose( position=Point(x=pose[0], y=pose[1], z=0), orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3]) ) # Publish pose stamped self.pose_est_pub.publish( PoseStamped( header=header, pose=pose ) ) self.br.sendTransform((self.pose_est[0], self.pose_est[1], .125), q, rospy.Time.now(), "base_link", "map") # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() covariance = np.array([.05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05, 0, 0, 0, 0, 0, 0, .05])**2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.pose = p_c self.p_c_s_est_pub.publish(p_c_s)
def state2pose(self, state, cov): robot_output = PoseWithCovarianceStamped() robot_output.pose.pose.position.x = state[0] robot_output.pose.pose.position.y = state[1] robot_cov = np.zeros((36,)) robot_cov[0] = cov[0,0] robot_cov[1] = cov[0,1] robot_cov[5] = cov[0,2] robot_cov[6] = cov[1,0] robot_cov[7] = cov[1,1] robot_cov[11] = cov[1,2] robot_cov[30] = cov[2,0] robot_cov[31] = cov[2,1] robot_cov[35] = cov[2,2] robot_output.pose.covariance = robot_cov.flatten() oppo = tf.transformations.quaternion_from_euler(0,0,state[2]) robot_output.pose.pose.orientation.x = oppo[0] robot_output.pose.pose.orientation.y = oppo[1] robot_output.pose.pose.orientation.z = oppo[2] robot_output.pose.pose.orientation.w = oppo[3] target_output = PoseWithCovarianceStamped() target_output.pose.pose.position.x = state[3] target_output.pose.pose.position.y = state[4] target_cov = np.zeros((36,)) target_cov[0] = cov[3,3] target_cov[1] = cov[3,4] target_cov[6] = cov[4,3] target_cov[7] = cov[4,4] target_output.pose.covariance = target_cov.flatten() h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'odom' robot_output.header = h target_output.header = h return target_output, robot_output
def poseMessage(self): newframe = "map" posemsg = PoseWithCovarianceStamped() poseStamped = PoseStamped() poseStamped.header = self.marker_pose.header poseStamped.pose = self.marker_pose.pose.pose try: poseStamped = self.transformer.transformPose(newframe,poseStamped) except tf.ConnectivityException as ex: rospy.logerr(ex) raise Exception("Transform failed") except tf.LookupException as ex: rospy.logerr(ex) raise Exception("Transform failed") except tf.ExtrapolationException as ex: rospy.logerr(ex) raise Exception("Transform failed") posemsg.header = poseStamped.header posemsg.pose.pose = poseStamped.pose # Pose posemsg.pose.covariance = self.marker_pose.pose.covariance return posemsg
def start_ekf(self, position): q = tf.transformations.quaternion_from_euler(0, 0, position[2]) header = Header( stamp = rospy.Time.now(), frame_id = "map" ) pose = Pose( position = Point( x = position[0], y = position[1], z = 0 ), orientation = Quaternion( x = q[0], y = q[1], z = q[2], w = q[3], ) ) # Publish pose with covariance stamped. p_c_s = PoseWithCovarianceStamped() p_c = PoseWithCovariance() # These don't matter covariance = np.array([.01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .0001]) ** 2 p_c.pose = pose p_c.covariance = covariance p_c_s.header = header p_c_s.header.frame_id = "map" p_c_s.pose = p_c self.set_init_pose(p_c_s)
def get_lsq_triangulation_error_with_noisy_gt(self,observation,epsilon=0.5): noisy_joints = self.get_noisy_joints() stamp = noisy_joints.header.stamp noisy_joints_neighbor = self.get_noisy_joints_neighbor(stamp) try: self.joints = np.array(noisy_joints.res).reshape((14,2)) self.joints_prev = self.joints except: self.joints = self.joints_prev print('Unable to find noisy joints') try: self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape((14,2)) self.joints_neighbor_prev = self.joints_neighbor except: self.joints_neighbor = self.joints_neighbor_prev print('Unable to find noisy joints from neighbor') # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows try: trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame self.trans_prev = trans self.rot_prev = rot except: trans = self.trans_prev rot = self.rot_prev print('Robot rotation unavailable') (r,p,y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H1[0:3,3] = trans try: trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', stamp) #target to source frame self.trans2_prev = trans self.rot2_prev = rot except: trans = self.trans2_prev rot = self.rot2_prev print('Robot neighbor rotation unavailable') (r,p,y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H2[0:3,3] = trans #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1,H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k in range(len(gt_joints)): p2d1 = self.joints[k,:] p2d2 = self.joints_neighbor[k,:] points_2d = np.array((p2d1,p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2) es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() #if v>4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position error += (self.get_distance_from_point(gt,es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[k].publish(err_cov) # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error/len(gt_joints) if error<=epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos,error]
def poseCallback(mag): pose = PoseWithCovarianceStamped() pose.header = mag.header pose.pose.orientation = Quaternion(mag.x, mag.y, mag.z) # TODO: https://cdn-shop.adafruit.com/datasheets/AN203_Compass_Heading_Using_Magnetometers.pdf pub.publish(pose)
''' def yaw_to_quat(yaw): """ Computing corresponding quaternion q to angle yaw [rad] :param yaw :return: q """ q = Quaternion(axis=[0, 0, 1], angle=yaw) return q.elements rospy.init_node('init_pos') pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1) initpose_msg = PoseWithCovarianceStamped() initpose_msg.header.frame_id = "map" initpose_msg.pose.pose.position.x = float(sys.argv[1]) initpose_msg.pose.pose.position.y = float(sys.argv[2]) quaternion = yaw_to_quat((float(sys.argv[3]))) initpose_msg.pose.pose.orientation.x = quaternion[0] initpose_msg.pose.pose.orientation.y = quaternion[1] initpose_msg.pose.pose.orientation.z = quaternion[2] initpose_msg.pose.pose.orientation.w = quaternion[3] rospy.sleep(1) rospy.loginfo("Setting initial pose") pub.publish(initpose_msg) print(initpose_msg) rospy.loginfo("Initial pose SET")
def execute_overhead_callback(self, goal): now = rospy.Time.now() if self.ros_overhead_cam_info is None: raise Exception("Camera Info not received") self.last_ros_image_stamp = self.ros_image_stamp try: self.ros_overhead_trigger.wait_for_service(timeout=0.1) self.ros_overhead_trigger() except: pass wait_count = 0 while self.ros_overhead_cam_info.ros_image is None or self.ros_image_stamp == self.last_ros_image_stamp: if wait_count > 250: raise Exception("Image receive timeout") time.sleep(0.25) wait_count += 1 print "Past timeout" #stored data in local function variable, so changed all references off of self.ros_image img = self.ros_overhead_cam_info.ros_image if img is None: raise Exception("Camera image data not received") print "image received" r_array = RecognizedObjectArray() r_array.header.stamp = now r_array.header.frame_id = self.frame_id if goal.use_roi: raise Warning("use_roi in ObjectRecognitionRequest ignored") search_objects = dict() with self.payloads_lock: for _, p in self.payloads.items(): search_objects[p.name] = p.markers for _, l in self.link_markers.items(): search_objects[l.header.frame_id] = l.markers #TODO: handle multiple aruco dictionaries, currently only use one print "past payload lock" aruco_dicts = set() for m in search_objects.itervalues(): for m2 in m: aruco_dicts.add(m2.marker.dictionary) print len(aruco_dicts) aruco_dicts.add("DICT_ARUCO_ORIGINAL") assert len( aruco_dicts ) == 1, "Currently all tags must be from the same dictionary" if not hasattr(cv2.aruco, next(iter(aruco_dicts))): raise ValueError("Invalid aruco-dict value") aruco_dict_id = getattr(cv2.aruco, next(iter(aruco_dicts))) aruco_dict = cv2.aruco.Dictionary_get(aruco_dict_id) c_pose = self.listener.lookupTransform( "/world", self.ros_overhead_cam_info.ros_data.header.frame_id, rospy.Time(0)) print "c_pose" print c_pose.p print c_pose.R parameters = cv2.aruco.DetectorParameters_create() parameters.cornerRefinementWinSize = 32 parameters.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers( img, aruco_dict, parameters=parameters) print ids for object_name, payload_markers in search_objects.items(): tag_object_poses = dict() for m in payload_markers: board = get_aruco_gridboard(m.marker, aruco_dict) #board = cv2.aruco.GridBoard_create(4, 4, .04, .0075, aruco_dict, 16) retval, rvec, tvec = cv2.aruco.estimatePoseBoard( corners, ids, board, self.ros_overhead_cam_info.camMatrix, self.ros_overhead_cam_info.distCoeffs) print retval print rvec print tvec if (retval > 0): if (m.name == "leeward_mid_panel_marker_1" or m.name == "leeward_tip_panel_marker_1"): print "Found tag: " + m.name try: #o_pose = self.listener.lookupTransform(m.name, object_name, rospy.Time(0)) o_pose = rox_msg.msg2transform(m.pose).inv() print object_name print o_pose print "" print "o_pose" print o_pose.p print o_pose.R Ra, b = cv2.Rodrigues(rvec) a_pose = rox.Transform(Ra, tvec) print "a_pose" print a_pose.p print a_pose.R tag_object_poses[m.name] = c_pose * (a_pose * o_pose) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue #TODO: merge tag data if multiple tags detected if len(tag_object_poses) > 0: p = PoseWithCovarianceStamped() p.pose.pose = rox_msg.transform2pose_msg( tag_object_poses.itervalues().next()) p.header.stamp = now p.header.frame_id = self.frame_id r = RecognizedObject() r.header.stamp = now r.header.frame_id = self.frame_id r.type.key = object_name r.confidence = 1 r.pose = p r_array.objects.append(r) """for o in self.object_names: try: (trans,rot) = self.listener.lookupTransform("/world", o, rospy.Time(0)) print o print trans print rot print "" p=PoseWithCovarianceStamped() p.pose.pose=Pose(Point(trans[0], trans[1], trans[2]), Quaternion(rot[0], rot[1], rot[2], rot[3])) p.header.stamp=now p.header.frame_id=self.frame_id r=RecognizedObject() r.header.stamp=now r.header.frame_id=self.frame_id r.type.key=o r.confidence=1 r.pose=p r_array.objects.append(r) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue""" print "Succeeded in finding tags" result = ObjectRecognitionResult() result.recognized_objects = r_array self.overhead_server.set_succeeded(result=result)
def __init__(self): rospy.init_node('exploring_random', anonymous=True) rospy.on_shutdown(self.shutdown) # 在每个目标位置暂停的时间 (单位:s) self.rest_time = rospy.get_param("~rest_time", 2) # 是否仿真? self.fake_test = rospy.get_param("~fake_test", True) # 到达目标的状态 goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # 设置目标点的位置 # 在rviz中点击 2D Nav Goal 按键,然后单击地图中一点 # 在终端中就会看到该点的坐标信息 locations = dict() locations['1'] = Pose(Point(4.589, -0.376, 0.000), Quaternion(0.000, 0.000, -0.447, 0.894)) locations['2'] = Pose(Point(4.231, -6.050, 0.000), Quaternion(0.000, 0.000, -0.847, 0.532)) locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)) locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940)) locations['6'] = Pose(Point(2.924, 0.018, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) # 发布控制机器人的消息 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # 订阅move_base服务器的消息 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # 60s等待时间限制 self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # 保存机器人的在rviz中的初始位置 initial_pose = PoseWithCovarianceStamped() # 保存成功率、运行时间、和距离的变量 n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # 确保有初始位置 while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # 开始主循环,随机导航 while not rospy.is_shutdown(): # 如果已经走完了所有点,再重新开始排序 if i == n_locations: i = 0 sequence = sample(locations, n_locations) # 如果最后一个点和第一个点相同,则跳过 if sequence[0] == last_location: i = 1 # 在当前的排序中获取下一个目标点 location = sequence[i] # 跟踪行驶距离 # 使用更新的初始位置 if initial_pose.header.stamp == "": distance = sqrt( pow( locations[location].position.x - locations[last_location].position.x, 2) + pow( locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt( pow( locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow( locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # 存储上一次的位置,计算距离 last_location = location # 计数器加1 i += 1 n_goals += 1 # 设定下一个目标点 self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # 让用户知道下一个位置 rospy.loginfo("Going to: " + str(location)) # 向下一个位置进发 self.move_base.send_goal(self.goal) # 五分钟时间限制 finished_within_time = self.move_base.wait_for_result( rospy.Duration(300)) # 查看是否成功到达 if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # 运行所用时间 running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # 输出本次导航的所有信息 rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
def fid_cb(msg): """ callback which takes fiducial transfroms and publishes pose of robot base relative to map @param msg: A fiducial_msgs FidcucialTransformArray object """ global tf_broadcaster global tf_listener global FIDUCIAL_NAMES global pose_pub # if driving, don't interupt if get_state() not in [States.LOST, States.TELEOP]: #removed docked state return # if fiducials found, take the first if len(msg.transforms) == 0: return transform = msg.transforms[0] # swap y and z axes to fit map frame of reference pos = transform.transform.translation rot = transform.transform.rotation pos.x, pos.y, pos.z = pos.x, pos.z, pos.y rot.x, rot.y, rot.z = rot.x, rot.z, rot.y transform.transform.translation = pos transform.transform.rotation = rot # invert the transform homo_mat = PoseConv().to_homo_mat(transform.transform) inverted_tf = PoseConv().to_tf_msg(np.linalg.inv(homo_mat)) # send a transform from camera to fiducial m = TransformStamped() m.transform = inverted_tf m.header.frame_id = FIDUCIAL_NAMES.get(str(transform.fiducial_id)) m.header.stamp = rospy.Time.now() m.child_frame_id = "fiducial_camera" tf_broadcaser.sendTransform(m) # calculate transform from map to base try: latest_time = tf_listener.getLatestCommonTime("/map","/fiducial_base") base_to_map = tf_listener.lookupTransform("/map","/fiducial_base",latest_time) except tf2_ros.TransformException: rospy.logwarn("failed to transform, is fiducial {} mapped?".format(transform.fiducial_id)) return # convert transform to PoseWithCovarianceStamped robot_pose = PoseConv().to_pose_msg(base_to_map) pose_w_cov_stamped = PoseWithCovarianceStamped() pose_w_cov_stamped.pose.pose = robot_pose pose_w_cov_stamped.header.stamp = rospy.Time.now() pose_w_cov_stamped.header.frame_id = "map" rospy.logdebug("Sending fiducial pose:\n{}".format(robot_pose)) # publish to /initialpose pose_pub.publish(pose_w_cov_stamped) # update state try: prev_state = get_state() if prev_state == States.LOST: change_state(States.WAITING) rospy.loginfo("Fiducial {} seen, no longer lost".format(transform.fiducial_id)) except rospy.ServiceException: rospy.logerr("Could not access state service")
def __init__(self): rospy.init_node('random_navigation', anonymous=True) rospy.on_shutdown(self.shutdown) # 在每个目标位置暂停的时间 self.rest_time = rospy.get_param("~rest_time", 2) # 到达目标的状态 goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # 设置目标点的位置 # 如果想要获得某一点的坐标,在rviz中点击 2D Nav Goal 按键,然后单机地图中一点 # 在终端中就会看到坐标信息 locations = dict() locations['p1'] = Pose(Point(1.150, 5.461, 0.000), Quaternion(0.000, 0.000, -0.013, 1.000)) locations['p2'] = Pose(Point(6.388, 2.66, 0.000), Quaternion(0.000, 0.000, 0.063, 0.998)) locations['p3'] = Pose(Point(8.089, -1.657, 0.000), Quaternion(0.000, 0.000, 0.946, -0.324)) locations['p4'] = Pose(Point(9.767, 5.171, 0.000), Quaternion(0.000, 0.000, 0.139, 0.990)) locations['p5'] = Pose(Point(0.502, 1.270, 0.000), Quaternion(0.000, 0.000, 0.919, -0.392)) locations['p6'] = Pose(Point(4.557, 1.234, 0.000), Quaternion(0.000, 0.000, 0.627, 0.779)) # 发布控制机器人的消息 self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # 订阅move_base服务器的消息 self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # 60s等待时间限制 self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # 保存机器人的在rviz中的初始位置 initial_pose = PoseWithCovarianceStamped() # 保存成功率、运行时间、和距离的变量 n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # 确保有初始位置 while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # 开始主循环,随机导航 if not rospy.is_shutdown(): # 在当前的排序中获取下一个目标点 location = 'p1' # 跟踪行驶距离 # 使用更新的初始位置 if initial_pose.header.stamp == "": distance = sqrt( pow( locations[location].position.x - locations[last_location].position.x, 2) + pow( locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt( pow( locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow( locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # 存储上一次的位置,计算距离 last_location = location # 计数器加1 i += 1 n_goals += 1 # 设定下一个目标点 self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # 让用户知道下一个位置 rospy.loginfo("Going to: " + str(location)) # 向下一个位置进发 self.move_base.send_goal(self.goal) # 五分钟时间限制 finished_within_time = self.move_base.wait_for_result( rospy.Duration(300)) # 查看是否成功到达 if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # 运行所用时间 running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # 输出本次导航的所有信息 rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
def __init__(self): rospy.init_node('nav_test', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 2) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] locations = dict() locations['A'] = Pose(Point(1.279, -0.265, 0.000), Quaternion(0.000, 0.000, 0.318, 0.948)) locations['厨房'] = Pose(Point(2.539, 1.657, 0.000), Quaternion(00.000, 0.000, 0.000, 1.000)) locations['卧室'] = Pose(Point(1.337, 1.090, 0.000), Quaternion(0.000, 0.000, 0.924, 0.382)) locations['D'] = Pose(Point(-0.324, -0.276, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" self.Dist = 0 self.Asr = "" Loc_1=0 Loc_2=0 Name_1=0 name_2=0 Name="" Drink_1=0 Drink_2=0 Drink_3=0 Drink="" # Get the initial pose from the user (how to set) rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") dict_keys = ['A','厨房','D','客厅'] pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5) pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32, queue_size=5) pub3 = rospy.Publisher('arm',String,queue_size=5) pub4 = rospy.Publisher('tracking',Int32,queue_size=5) pub5 = rospy.Publisher('/following',Int32,queue_size=5) pub6 = rospy.Publisher('drink_type',String,queue_size=5) pub7 = rospy.Publisher('ros2_wake',Int32,queue_size=5) ####################################### # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): if i == n_locations: i = 0 # sequence = sample(locations, n_locations) # Skip over first location if it is the same as # the last location if dict_keys[0] == last_location: i = 1 ##################################### ####### Modify by XF 2017.4.14 ###### # location = sequence[i] # rospy.loginfo("location= " + str(location)) # location = locations.keys()[i] location = dict_keys[i] ##################################### # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt(pow(locations[location].position.x - locations[last_location].position.x, 2) + pow(locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt(pow(locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow(locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations last_location = location # Increment the counters i += 1 n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) # Allow 6 minutes to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(360)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.ABORTED: # can not find a plan,give feedback rospy.loginfo("please get out") status_n = "please get out" pub1.publish(status_n) if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) ############ add voice by XF 4.25 ################## The 1st # pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5) loc ="我已到达"+str(location) pub1.publish(loc) rospy.sleep(5) rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly rospy.loginfo("Dist:"+str(self.Dist)) if str(location) == 'A': commandA1="您好,有什么可以帮助您?" pub1.publish(commandA1) ############# now next asr ###################### awake=1 pub2.publish(awake) rospy.Subscriber('/voice/xf_asr_follow',String,self.callbackAsr) ###### Modify by XF 2017. 4.21 ######### rospy.sleep(15) rospy.loginfo("self.Asr:"+str(self.Asr)) Loc_1=string.find(self.Asr,'卧室') Loc_2=string.find(self.Asr,'客厅') Name_1=string.find(self.Asr,'张三') Name_2=string.find(self.Asr,'李四') Drink_1=string.find(self.Asr,'可乐') Drink_2=string.find(self.Asr,'雪碧') Drink_3=string.find(self.Asr,'橙汁') if Loc_1!=-1: rospy.loginfo("匹配卧室") dict_keys = ['A','厨房','卧室','D'] if Loc_2!=-1: rospy.loginfo("匹配客厅") dict_keys = ['A','厨房','客厅','D'] if Name_1!=-1: Name = '张三' rospy.loginfo("匹配张三") if Name_2!=-1: Name = '李四' rospy.loginfo("匹配李四") if Drink_1!=-1: Drink = '可乐' if Drink_2!=-1: Drink = '雪碧' if Drink_3!=-1: Drink = '橙汁' ros2_wake=1 pub7.publish(ros2_wake) pub6.publish(Drink) ####################################### rospy.sleep(5) commandA2="我已获取任务,请稍等." pub1.publish(commandA2) ################################################## The 2nd if str(location) == '厨房': ############## add by XF 5.27 ################## rospy.sleep(5) track=1 pub4.publish(track) rospy.sleep(10) rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly rospy.loginfo("Dist2:"+str(self.Dist)) rospy.loginfo("go away Dist:"+str(self.Dist/1000.0-0.3)) self.goal = MoveBaseGoal() self.goal.target_pose.pose.position.x = self.Dist/1000.0-0.3 self.goal.target_pose.pose.orientation.w = 1.0; self.goal.target_pose.header.frame_id = 'base_footprint' self.goal.target_pose.header.stamp = rospy.Time.now() self.move_base.send_goal(self.goal) rospy.sleep(5) ################################################ commandB1="pick" pub3.publish(commandB1) rospy.sleep(10) commandB2="我已找到"+str(Drink)+"并抓取." pub1.publish(commandB2) ################################################## The 3rd if str(location) == '卧室': # rospy.sleep(5) follow=1 pub5.publish(follow) track=2 #stop tracking pub4.publish(track) rospy.loginfo("send stop tracking command") #subscribe the angle # rospy.Subscriber('xf_xfm_node', Int32, self.callbackXFM) rospy.sleep(10) commandC2=str(Name)+"给您"+str(Drink) pub1.publish(commandC2) rospy.sleep(3) commandC1="release" pub3.publish(commandC1) rospy.sleep(50) ################################################## else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
def futureSim(passed_roomba_state, passed_actions, start_pose): """ This simulation is for short term rapid location estimation of a Roomba that hasn't been seen for a while. 20 hypothetical "Imaginary Roombas" trace out paths it could have taken, and the centroid of the final scatter determines the hypothesized final position of the actual Roomba. times_since = Array. Time since the Roomba was last seen. :arg (rospy.Time) last_turn: The last time the Roomba turned every 20 seconds :arg (rospy.Time) last_noise: The last time the Roomba had noise :arg (PoseWithCovarianceStamped) start_pose: The start pose of the Roomba :arg (rospy.Time) end_time: When we want to simulate until :returns prediction: The estimated final position :rtype: PoseWithCovarianceStamped """ (last_turn, last_noise, end_time) = passed_roomba_state roomba_pos = [(0, 0)] * 20 # Generate 20 hypothetical Roombas' positions known_time = start_pose.header.stamp sim_duration = (end_time - known_time).to_sec() start_pos = start_pose.pose.pose.position quaternion = start_pose.pose.pose.orientation quaternion_arr = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] start_orient = tf.transformations.euler_from_quaternion(quaternion_arr)[2] # Check time remaining until Roomba experiences noise time_to_noise = 5 - (known_time - last_noise).to_sec() % 5 # Check number of times Roombas should go through "noise" noise_cycles = int(math.ceil((sim_duration - time_to_noise) / 5.0)) # Check time remaining until Roomba turns time_to_turn = 20 - (known_time - last_turn).to_sec() % 20 # Check number of times Roombas should go through "turn cycles" turn_cycles = int(math.ceil((sim_duration - time_to_turn) / 20.0)) event_queue = populateQueue(passed_actions,(turn_cycles,time_to_turn),(noise_cycles,time_to_noise)) # For one of 20 hypothetical roombas... for roomba in range(0, len(roomba_pos)): print "Roomba %i" % roomba orient = start_orient prev_time = 0.0 roomba_pos[roomba] = start_pos.x, start_pos.y, orient for item in event_queue: (event, time_to_event) = item assert (time_to_event >= prev_time) or ((event is Action.TURN) or (event is Action.NOISE) or (event is Action.COLLISION)), \ "Event time error. %r occurs too rapidly after previous event." % (event) if time_to_event - prev_time >= 0: roomba_pos[roomba] = locCalc(roomba_pos[roomba], time_to_event - prev_time, orient) (orient, event_length) = performEvent(orient, event) roomba_pos[roomba] = locCalc(roomba_pos[roomba], 0.0, orient) prev_time = time_to_event + event_length # rospy.loginfo_throttle("Simulated positions: {}".format(roomba_pos)) mean_pos = np.mean(roomba_pos, axis=0) # print mean_pos mean_x = mean_pos[0] mean_y = mean_pos[1] mean_quaternion = tf.transformations.quaternion_from_euler(0, 0, mean_pos[2]) msg = Pose(position=Point(mean_x, mean_y, 0), orientation=Quaternion(mean_quaternion[0], mean_quaternion[1], mean_quaternion[2], mean_quaternion[3])) # TODO: Calculate variance using eigenvectors of covariance matrix pad = np.zeros(20) XYZRPY = [np.array(roomba_pos)[:,0],np.array(roomba_pos)[:,1],pad,pad,pad,np.array(roomba_pos)[:,2]] cov = np.cov(XYZRPY) print cov # eigvals, eigvecs = np.linalg.eig(covariance) # print "Eigenvalues", eigvals # print "Eigenvectors", eigvecs return PoseWithCovarianceStamped( header=start_pose.header, pose=PoseWithCovariance( pose=msg, covariance=cov ) )
#!/usr/bin/env python import rospy from geometry_msgs.msg import PoseWithCovarianceStamped import time if __name__ == "__main__": rospy.init_node("initial_pose_publisher_script") pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=10) print("Created publisher.") rospy.sleep(1.0) msg = PoseWithCovarianceStamped() msg.pose.pose.position.x = 71.86 msg.pose.pose.position.y = 97.99 pub.publish(msg) print("Published message.") rospy.spin()
def __init__(self): # Give the node a name rospy.init_node('ccam_navigation', anonymous=True) # Set rospy to execute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # The minimum time the robot should pause at each location self.rest_time = rospy.get_param("~rest_time", 5) # Goal state return values goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # Pose coordinates are then displayed in the terminal # that was used to launch RViz. locations = dict() locations['start'] = Pose(Point(0.000, 0.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['5m'] = Pose(Point(5.000, 0.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['10m'] = Pose(Point(10.000, -0.525, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['20m'] = Pose(Point(20.000, -1.000, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['40m'] = Pose(Point(40.000, -1.250, 0.000), Quaternion(0.000, 0.000, 1.000, 0.000)) locations['drill_start'] = Pose(Point(0.400, 0.000, 0.000), Quaternion(0.000, 0.000, -0.700, 0.700)) locations['seal_start'] = Pose(Point(-0.600, 0.000, 0.000), Quaternion(0.000, 0.000, 0.700, 0.700)) # Publisher to manually control the robot (e.g. to stop it, queue_size=5) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Get the initial pose from the user rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # Begin the main loop and navigation to user specified locations while not rospy.is_shutdown(): location = raw_input('Enter desired location (ie. start, drill_station_1, etc.): ') if location == "start" or location == "5m" or location == "10m" or location == "20m" or location == "40m" or location == "drill_start" or location == "seal_start": # Set up the goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward user specified location self.move_base.send_goal(self.goal) # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Reached " + str(location)) rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) rospy.sleep(self.rest_time) else: print "Unknown location selected. Please select a know area."
#!/usr/bin/env python import rospy from goal_publisher.msg import PointArray import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult from nav_msgs.srv import GetPlan, GetPlanRequest, GetPlanResponse import time from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped import numpy as np from operator import itemgetter rospy.init_node('goal_navigation') reach = list() pos_goal = list() pos_curr = PoseWithCovarianceStamped() """ Getting current position. """ def amcl_mycallback(msg): global pos_curr pos_curr = msg """Sorting goal positions in descending order. Note that, by changing the reverse = false , goals can be sorted in ascending order. """ def goal_sorting(point):
def __init__(self): rospy.on_shutdown(self.cleanup) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") pub.publish("Waiting for move_base action server...") # Wait for the action server to become available self.move_base.wait_for_server(rospy.Duration(120)) rospy.loginfo("Connected to move base server") pub.publish("Connected to move base server") # A variable to hold the initial pose of the robot to be set by the user in RViz initial_pose = PoseWithCovarianceStamped() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Get the initial pose from the user rospy.loginfo("Click the 2D Pose Estimate button in RViz to set the robot's initial pose") pub.publish("Click the 2D Pose Estimate button in RViz to set the robot's initial pose") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) point_pose = String() rospy.Subscriber('lm_data', String, self.update_point_pose) while True: rospy.loginfo("Waiting for your order...") pub.publish("Waiting for your order...") rospy.sleep(3) rospy.wait_for_message('lm_data', String) if(self.point_pose.data == "GO TO POINT A"): A_x = 1.8 A_y = -5.51 A_theta = 1.5708 elif(self.point_pose.data == "GO TO POINT B"): B_x = -2.2 B_y = -2.53 B_theta = 1.5708 elif(self.point_pose.data == "GO TO POINT C"): C_x = 1.9 C_y = 1.18 C_theta = 1.5708 rospy.loginfo("Ready to go") pub.publish("Ready to go") rospy.sleep(1) locations = dict() quaternion = quaternion_from_euler(0.0, 0.0, A_theta) locations['A'] = Pose(Point(A_x, A_y, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) self.goal = MoveBaseGoal() rospy.loginfo("Starting navigation test") self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() rospy.loginfo("Going to point "+A_name) pub.publish("Going to point "+A_name) rospy.sleep(2) self.goal.target_pose.pose = locations['A'] self.move_base.send_goal(self.goal) waiting = self.move_base.wait_for_result(rospy.Duration(300)) if waiting == 1: rospy.loginfo("Reached point "+A_name) pub.publish("Reached point "+A_name) rospy.sleep(2)
def __init__(self): rospy.init_node('nav_test', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 2) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # Pose coordinates are then displayed in the terminal # that was used to launch RViz. locations = dict() locations['1'] = Pose(Point(45,600, -12,000, 0,000), Quaternion(0,000, 0,000, 0,000, 1,000)) locations['2'] = Pose(Point(60,500, -12,000, 0,000), Quaternion(0,000, 0,000, -0,700, 0,714)) locations['3'] = Pose(Point(60,500, -17,000, 0,000), Quaternion(0,000, 0,000, 1,000, 0,000)) locations['4'] = Pose(Point(45,600, -17,000, 0,000), Quaternion(0,000, 0,000, 0,710, 0,704)) locations['5'] = Pose(Point(45,600, -12,500, 0,000), Quaternion(0,000, 0,000, -0,010, 1,000)) locations['6'] = Pose(Point(60,500, -12,500, 0,000), Quaternion(0,000, 0,000, -0,715, 0,699)) locations['7'] = Pose(Point(60,500, -13,500, 0,000), Quaternion(0,000, 0,000, 1,000, -0,007)) locations['8'] = Pose(Point(45,600, -13,500, 0,000), Quaternion(0,000, 0,000, -0,710, 0,704)) locations['9'] = Pose(Point(45,600, -14,500, 0,000), Quaternion(0,000, 0,000, -0,003, 1,000)) locations['10'] = Pose(Point(60,500, -14,500, 0,000), Quaternion(0,000, 0,000, -0,705, 0,710)) locations['11'] = Pose(Point(60,500, -15,500, 0,000), Quaternion(0,000, 0,000, 1,000, 0,002)) locations['12'] = Pose(Point(45,600, -15,500, 0,000), Quaternion(0,000, 0,000, -0,711, 0,704)) locations['13'] = Pose(Point(45,600, -16,500, 0,000), Quaternion(0,000, 0,000, -0,019, 1,000)) locations['14'] = Pose(Point(60,500, -16,500, 0,000), Quaternion(0,000, 0,000, -0,714, 0,701)) locations['15'] = Pose(Point(60,500, -17,000, 0,000), Quaternion(0,000, 0,000, 1,000, 0,002)) locations['16'] = Pose(Point(45,600, -17,000, 0,000), Quaternion(0,000, 0,000, 0,720, 0,693)) locations['17'] = Pose(Point(45,600, -12,000, 0,000), Quaternion(0,000, 0,000, 0,000, 1,000)) #locations['midten'] = Pose(Point(-42.651, 24.841, 0.000), Quaternion(0.000, 0.000,-0.734, 0.679)) #locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451)) # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # Get the initial pose from the user #rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") #rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) #self.last_location = Pose() #rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose #while initial_pose.header.stamp == "": # rospy.sleep(1) rospy.loginfo("Starting navigation test") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): # If we've gone through the current sequence, # start with a new random sequence if i == n_locations: i = 0 sequence = ['1','2','3','4','5','6','7','8','9','10','11','12','13','14','15','16','17'] # Skip over first location if it is the same as # the last location if sequence[0] == last_location: i = 1 print sequence # Get the next location in the current sequence location = sequence[i] # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt(pow(locations[location].position.x - locations[last_location].position.x, 2) + pow(locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt(pow(locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow(locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations last_location = location # Increment the counters i += 1 n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
def _get_obs(self): """ Read robot state :return: """ rospy.logdebug("Start Get Observation ==>") # Pose of the MAV pose = self.get_gt_odom() stamp = pose.header.stamp # Velocity of the MAV vel = self.get_velocity(stamp) # Position of the Target target = self.get_gt_target(stamp) ################################################################################################### ''' Inputs to Value Network ''' ################################################################################################### '''Translation of Actor Joints w.r.t world''' gt = [] for k in range(len(gt_joints)): try: (trans,rot) = self.listener.lookupTransform( 'world_ENU',gt_joints_names[k], stamp) gt.extend([trans[0],trans[1],trans[2]]) except: (trans,rot) = self.listener.lookupTransform( 'world_ENU',gt_joints_names[k], rospy.Time(0)) gt.extend([trans[0],trans[1],trans[2]]) '''Actor Root Orientation in World frame: Input to Value Function ''' try: (trans,rot) = self.listener.lookupTransform('world_ENU','actor::Head', stamp) except: (trans,rot) = self.listener.lookupTransform('world_ENU','actor::Head', rospy.Time(0)) (r,p,y) = tf.transformations.euler_from_quaternion(rot) target_orient = y '''MAV Orientation in World frame: Input to Value Function ''' try: (trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', stamp) self.yaw1rot_prev = rot except: (trans,rot) = self.listener.lookupTransform( 'world_ENU',self.rotors_machine_name+'/base_link', rospy.Time(0)) print('Unable to find yaw1') (r,p,y) = tf.transformations.euler_from_quaternion(rot) yaw1 = y ################################################################################################### ################################################################################################### ''' Inputs for Policy Network ''' ################################################################################################### '''Actor Root in Mav base link frame: Only Translation. Also used for calculating MAV bearing''' try: (trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::actor_pose', stamp) self.target1_prev = trans self.target1rot_prev = rot except: try: trans=self.target1_prev rot=self.target1rot_prev except: (trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::actor_pose', rospy.Time(0)) print('Unable to find target:'+self.rotors_machine_name) theta1 = np.arctan2(trans[1],trans[0]) (r,p,y) = tf.transformations.euler_from_quaternion(rot) delta_target1 = PoseWithCovarianceStamped(); delta_target1.pose.pose.position.x = trans[0];delta_target1.pose.pose.position.y = trans[1];delta_target1.pose.pose.position.z = trans[2] '''Actor Root Orientation in Mav base link frame: Only Rotation ''' try: (trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::Head', stamp) except: (trans,rot) = self.listener.lookupTransform(self.rotors_machine_name+'/base_link','actor::Head', rospy.Time(0)) (r,p,y) = tf.transformations.euler_from_quaternion(rot) delta_orient = y '''Calculate relative velocity vector in the MAV base link frame''' homogeneous_matrix = tf.transformations.euler_matrix(0,0,yaw1,axes='sxyz') homogeneous_matrix[0:3,3] = [0,0,0] relative_vel = np.dot(np.linalg.inv(homogeneous_matrix), np.array([vel.twist.twist.linear.y - target.twist.twist.linear.x, vel.twist.twist.linear.x - target.twist.twist.linear.y,\ -vel.twist.twist.linear.z - target.twist.twist.linear.z,1])) #Dummy zero inputs to value network during testing. value_input = [0]*len(list(gt)+[pose.position.x,pose.position.y,pose.position.z,target.twist.twist.linear.x,target.twist.twist.linear.y,target.twist.twist.linear.z,vel.twist.twist.linear.x,vel.twist.twist.linear.y,-vel.twist.twist.linear.z,np.cos(yaw1), np.sin(yaw1),np.cos(target_orient), np.sin(target_orient)]) scale = 2 # Policy Inputs + Value Inputs observations = [scale*delta_target1.pose.pose.position.x, scale*delta_target1.pose.pose.position.y, scale*delta_target1.pose.pose.position.z,\ scale*relative_vel[0],scale*relative_vel[1],scale*relative_vel[2],np.cos(theta1), np.sin(theta1),np.cos(delta_orient), np.sin(delta_orient)]+value_input rospy.loginfo("Observations==>"+str(observations)) rospy.loginfo("END Get Observation ==>") return observations
abs(initial_position.pose.pose.orientation.w - current_pose.pose.pose.orientation.w) < margin ): return True else: return False # ---------------------------------------------------------------------------- # # Setup Node and variables # # ---------------------------------------------------------------------------- # rospy.init_node('Initial_pose', anonymous=False) rospy.Subscriber('/odom', Odometry, current_pose_callback) pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size = 10) rate = rospy.Rate(1) current_pose = PoseWithCovarianceStamped() checkpoint = PoseWithCovarianceStamped() margin = 0.2 # ---------------------------------------------------------------------------- # # Initialize setpoint # # ---------------------------------------------------------------------------- # checkpoint.pose.pose.position.x = 0.0#-2.0 checkpoint.pose.pose.position.y = 0.0#-0.5 checkpoint.pose.pose.position.z = 0.0 [x,y,z,w]=quaternion_from_euler(0.0,0.0,0.0) checkpoint.pose.pose.orientation.x = x checkpoint.pose.pose.orientation.y = y checkpoint.pose.pose.orientation.z = z checkpoint.pose.pose.orientation.w = w
def __init__(self): rospy.init_node('position_nav_node', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 3) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # # Pose coordinates are then displayed in the terminal # that was used to launch RViz. locations = dict() #locations['one-1'] = Pose(Point(1.1952,-0.1422,0.0), Quaternion(0.0,0.0,0.1744,0.9847)) #locations['one'] = Pose(Point(1.86,-1.247,0.0), Quaternion(0.0,0.0,0.8481,0.5298)) # locations['two-1'] = Pose(Point(1.3851,-0.1253,0.0), Quaternion(0.0,0.0,0.9818,-0.1901)) # locations['two-2'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337)) # locations['two'] = Pose(Point(-0.0019,-1.9595,0.0), Quaternion(0.0,0.0,-0.5236,0.8520)) # locations['three-1'] = Pose(Point(-0.4032,-0.9106,0.0), Quaternion(0.0,0.0,-0.5523,0.8337)) # locations['three-2'] = Pose(Point(0.2382,0.0559,0.0), Quaternion(0.0,0.0,0.2464,0.9692)) locations['three'] = Pose(Point(1.0602, 0.5279, 0.0), Quaternion(0.0, 0.0, 0.2534, 0.9674)) # locations['four'] = Pose(Point(1.0602,0.5279,0.0), Quaternion(0.0,0.0,0.2534,0.9674)) locations['three-2'] = Pose(Point(0.2382, 0.0559, 0.0), Quaternion(0.0, 0.0, 0.2464, 0.9692)) #initialpose locations['initial'] = locations['three-2'] # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) self.IOTnet_pub = rospy.Publisher('/IOT_cmd', IOTnet, queue_size=10) self.initial_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = 0 distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" sequeue = ['three'] # Get the initial pose from the user #rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") #rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) setpose = PoseWithCovarianceStamped( Header(0, rospy.Time(), "map"), PoseWithCovariance(locations['initial'], [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942 ])) self.initial_pub.publish(setpose) # Make sure we have the initial pose rospy.sleep(1) while initial_pose.header.stamp == "": rospy.sleep(1) rospy.sleep(1) # locations['back'] = Pose() rospy.loginfo("Starting position navigation ") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): # If we've gone through the all sequence, then exit if i == n_locations: rospy.logwarn("Now reach all destination, now exit...") rospy.signal_shutdown('Quit') break # Get the next location in the current sequence location = sequeue[i] # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt( pow( locations[location].position.x - locations[last_location].position.x, 2) + pow( locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt( pow( locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow( locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations last_location = location # Increment the counters i += 1 n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) #move_base.send_goal() # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(300)) # Map to 4 point nav cur_position = -1 position_seq = ['one'] if str(location) in position_seq: cur_position = position_seq.index(str(location)) + 1 # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() #move_base.cancle_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() #move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) # if cur_position!=-1: # self.IOTnet_pub.publish(cur_position) # rospy.sleep(12) # if cur_position == 2: # rospy.sleep(5) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # if cur_position != -1: # self.IOTnet_pub.publish(cur_position) # rospy.sleep(12) # if cur_position == 2: # rospy.sleep(5) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
class Car: global console_serG poseFromAmcl = PoseWithCovarianceStamped() # manually defined waypoints waypoint_x = [ 9.22297477722, 9.5609331131, 9.82821846008, 10.0760278702, 10.3985872269, 10.8705215454, 11.1512451172, 11.4630308151, 11.7872953415, 12.0715732574, 12.5092477798, 12.9083614349, 12.959431648, 13.0315103531, 13.0642805099, 13.0588378906, 13.0395879745 ] waypoint_y = [ 0.0268885232508, 0.00712374784052, -0.00839765090495, -0.0787715613842, -0.046705879271, -0.085138015449, -0.136199742556, -0.169776380062, -0.0992393344641, -0.135954737663, -0.112006239593, 0.361669361591, 0.636179447174, 0.964450240135, 1.27673280239, 1.62345945835, 1.95718026161 ] my_x = 0 my_y = 0 waypoint_index = 0 ## run_itr = 0 ## PID_steer oy y parameters : dist_goal = 0.0 err_old1 = 0 err1 = 0 err_sum1 = 0 d_g = 0.0 Kp1 = 25 Kd1 = 1 Ki1 = 0 y_low = -10 y_high = 10 ## PID_fwd or x parameters dist_goal = 0.0 err_old2 = 0 err2 = 0 err_sum2 = 0 d_g = 0.0 Kp2 = 10 Kd2 = 0 Ki2 = 0 x_low = -20 x_high = 20 ## car hardware initializations mode = 'A' ## imu initializations del_t = 0.0067 # for 150 Hz val_dx = 0 val_dy = 0 val_dz = 0 val_vx = 0 val_vy = 0 val_vz = 0 val_ax = 0 val_ay = 0 val_az = 0 bias_ax = 0 ## control the heading; y(or steer) then x(or fwd) def run(self): val_x = 0 val_y = 0 waypoint_number = 9 # choose val_x and val_y as per waypoint index val_x = self.waypoint_x[self.waypoint_index] val_y = self.waypoint_y[self.waypoint_index] # log iterations self.run_itr = self.run_itr + 1 flag = True print('before IMU') # use imu to uypdate my_x and my_y console_serG.write('IMU1') while not rospy.is_shutdown() and flag == True: char = console_serG.read() print('reading char') if char == 'I': print('inside if') self.val_ax = console_serG.read(6) self.val_ax = float(self.val_ax) self.val_ax = self.val_ax / 32768 * 2 * 9.8 self.val_ax_cor = self.val_ax - self.bias_ax self.val_vx = self.val_vx + self.val_ax_cor * self.del_t self.val_dx = self.val_dx + self.val_vx * self.del_t + self.val_ax * self.del_t * self.del_t * 0.5 print(' ax_cor:{} \t ax:{} \t dx:{}'.format( self.val_ax_cor, self.val_ax, self.val_dx)) flag = False print('imu complete') self.my_x = self.val_dx self.err1 = val_y - self.my_y y_cor = self.Kp1 * (self.err1) + self.Kd1 * ( self.err1 - self.err_old1) + self.Ki1 * (self.err_sum1) # bracket the y_cor or steer if y_cor < self.y_low: y_cor = self.y_low elif y_cor > self.y_high: y_cor = self.y_high # scale to -2048 to 2048 self.steer = y_cor / self.y_high * 2048 self.steer = int(self.steer) # note the steer_direction for string formingw if self.steer < 0 or self.steer == 0: steer_direction = '+' self.steer_abs = abs(self.steer) elif self.steer > 0: steer_direction = '-' self.steer_abs = abs(self.steer) self.steer = self.int_to_str_steer(self.steer_abs) self.err_sum1 = self.err1 + self.err_sum1 self.err_old1 = self.err1 self.err2 = val_x - self.my_x x_cor = self.Kp2 * (self.err2) + self.Kd2 * ( self.err2 - self.err_old2) + self.Ki2 * (self.err_sum2) # bracket the x_cor or fwd if x_cor < self.x_low: x_cor = self.x_low elif x_cor > self.x_high: x_cor = self.x_high # scale to -2048 to 2048 self.fwd = x_cor / self.x_high * 2048 self.fwd = int(self.fwd) # note the direction for string forming if self.fwd < 0 or self.fwd == 0: fwd_direction = '-' self.fwd_abs = abs(self.fwd) # fwd values cannot be negative self.fwd_abs = 0 elif self.steer > 0: fwd_direction = '+' self.fwd_abs = abs(self.fwd) if self.fwd_abs < 300 and self.fwd != 0: # drive motor dead self.fwd_abs = 300 self.fwd = self.int_to_str_fwd(self.fwd_abs) self.err_sum2 = self.err2 + self.err_sum2 self.err_old2 = self.err2 cmd = self.mode + steer_direction + self.steer + fwd_direction + self.fwd print("cmd:{}='A+s+f'| /pose x = {}, y = {} || wp_no: {}, itr: {}". format(cmd, self.my_x, self.my_y, self.waypoint_index, self.run_itr)) console_serG.write(cmd) print('command writing complete') ## This function takkes an integer and converts it into a string. It also adds leading zeros to make sure the string is in 4 digit format. The number entered in this function must be >=0. def int_to_str_steer(self, steer_abs): my_int = self.steer_abs if my_int > 999: my_int = str(my_int) elif (my_int < 999 or my_int == 999) and (my_int > 100 or my_int == 100): my_int = '0' + str(my_int) elif my_int < 100 and my_int > 9: my_int = '00' + str(my_int) elif my_int < 9 and my_int > 0: my_int = '000' + str(my_int) elif my_int == 0: my_int = '0000' my_int = str(my_int) return my_int def int_to_str_fwd(self, fwd_abs): my_int = self.fwd_abs if my_int > 999: my_int = str(my_int) elif (my_int < 999 or my_int == 999) and (my_int > 100 or my_int == 100): my_int = '0' + str(my_int) elif my_int < 100 and my_int > 9: my_int = '00' + str(my_int) elif my_int < 9 and my_int > 0: my_int = '000' + str(my_int) elif my_int == 0: my_int = '0000' my_int = str(my_int) return my_int
class GetGoal: #Initial pose pose_stamped=PoseWithCovarianceStamped() pose_stamped.pose.pose.position.x=5.0 pose_stamped.pose.pose.position.y=5.0 pose_stamped.pose.pose.orientation.z=0 map=OccupancyGrid() map_array=[[i for i in range(4000)],[j for j in range(4000)]] #map_array=[[],[]] Goal_is_Obtained = False def __init__(self): # Creates a node with rospy.init_node('GetGoal', anonymous=True) # Publisher which will publish to the topic 'Goal'. self.goal_publisher = rospy.Publisher('/move_base/goal', MoveBaseActionGoal, queue_size=10) # A subscriber to the topics 'cmd_vel'. #self.velocity_subscriber=rospy.Subscriber("/cmd_vel",Twist, self.timer_callback) # When receiving a message, call timer_callback() self.velocity_subscriber=rospy.Subscriber("/cmd_vel",Twist) # A subscriber to the topics '/amcl_Pose' self.pose_subscriber = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.update_pose) # A subscriber to the topics '/map' self.pose_subscriber = rospy.Subscriber('/map', OccupancyGrid, self.update_map) def timer_callback(self, msg): global timer print("Velocity message received") timer.cancel() timer = threading.Timer(2,self.PublishGoal) #2 seconds elapse timer.start() return def update_pose(self, msg): print("Map message recieved") self.pose_stamped=msg #self.pose_stamped.pose.pose.position.x #self.pose_stamped.pose.pose.position.y #self.pose_stamped.pose.pose.orientation.z def update_map(self, msg): print("Map message recieved") self.map=msg if not self.map.data: print("Map data is empty"); return if self.map.info.width == 640: return w=self.map.info.width h=self.map.info.heights for i in range(h): for j in range(w): try: self.map_array[i][j]=self.map.data[i*w+j-1] except IndexError: #print (i,j,self.map_array[i][j],self.map.data[i*w+j-1]) adress to self.map_array cause IndexError(out of range) self.PublishGoal def PublishGoal(self): x=self.pose_stamped.pose.pose.position.x y=self.pose_stamped.pose.pose.position.y theta=self.pose_stamped.pose.pose.orientation.z*PI #res=self.map.info.resolution res=0.05 i=3 sum=0 self.Goal_is_Obtained = True xc=int(x/res)-1 #x-check coordinate yc=int(y/res)-1 #map analysys while self.Goal_is_Obtained: #checks 8 sqrs around current position try: if self.check_sqr(xc+i, yc) < sum: sum=self.check_sqr(xc+i , yc); xg=xc+i; yg=yc; if self.check_sqr(xc-i, yc) < sum: sum=self.check_sqr(xc-i , yc); xg=xc-i; yg=yc; if self.check_sqr(xc, yc+i) < sum: sum=self.check_sqr(xc , yc+i); xg=xc; yg=yc+i; if self.check_sqr(xc, yc-i) < sum: sum=self.check_sqr(xc+i , yc-i); xg=xc; yg=yc-i; if self.check_sqr(xc+i, yc+i) < sum: sum=self.check_sqr(xc+i , yc+i); xg=xc+i; yg=yc+i; if self.check_sqr(xc+i, yc-i) < sum: sum=self.check_sqr(xc+i , yc-i); xg=xc+i; yg=yc-i; if self.check_sqr(xc-i , yc+i) < sum: sum=self.check_sqr(xc-i , yc+i); xg=xc-i; yg=yc+i; if self.check_sqr(xc-i , yc-i) < sum: sum=self.check_sqr(xc-i , yc-i); xg=xc-i; yg=yc-i; if sum < 0: self.Goal_is_Obtained = False else: i=i+1 except IndexError: pass #publishing goal print("Publishing goal") pose_msg=MoveBaseActionGoal pose_msg.goal.pose.position.x = xg*res pose_msg.goal.pose.position.y = yg*res #print(xg*res, yg*res) self.goal_publisher.pubish(pose_msg) return def check_sqr(self,x,y): #asssumes sqr 3x3 is known or not summurazing coast map #self.map_array=self.map.data sum=self.map_array[x][y]+self.map_array[x+1][y]+self.map_array[x][y+1]+self.map_array[x-1][y]+self.map_array[x][y-1]+self.map_array[x+1][y+1]+self.map_array[x+1][y-1]+self.map_array[x-1][y+1]+self.map_array[x-1][y-1] return sum
def get_lsq_triangulation_error(self,observation,epsilon=0.5): # Get the camera intrinsics of both cameras P1 = self.get_cam_intrinsic() P2 = self.get_neighbor_cam_intrinsic() # Convert world coordinates to local camera coordinates for both cameras # We get the camera extrinsics as follows trans,rot = self.listener.lookupTransform(self.rotors_machine_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame (r,p,y) = tf.transformations.euler_from_quaternion(rot) H1 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H1[0:3,3] = trans print(str(self.env_id)+' '+str(self.rotors_machine_name)+':'+str(trans)) trans,rot = self.listener.lookupTransform(self.rotors_neighbor_name+'/xtion_rgb_optical_frame','world', rospy.Time(0)) #target to source frame (r,p,y) = tf.transformations.euler_from_quaternion(rot) H2 = tf.transformations.euler_matrix(r,p,y,axes='sxyz') H2[0:3,3] = trans print(str(self.env_id)+' '+str(self.rotors_neighbor_name)+':'+str(trans)) #Concatenate Intrinsic Matrices intrinsic = np.array((P1[0:3,0:3],P2[0:3,0:3])) #Concatenate Extrinsic Matrices extrinsic = np.array((H1,H2)) joints_gt = self.get_joints_gt() error = 0 self.triangulated_root = PoseArray() for k,v in dict_joints.items(): p2d1 = self.joints[dict_joints[k],0:2] p2d2 = self.joints_neighbor[dict_joints[k],0:2] points_2d = np.array((p2d1,p2d2)) # Solve system of equations using least squares to estimate person position in robot 1 camera frame estimate_root = self.lstsq_triangulation(intrinsic,extrinsic,points_2d,2) es_root_cam = Point();es_root_cam.x = estimate_root[0];es_root_cam.y = estimate_root[1];es_root_cam.z = estimate_root[2] err_cov = PoseWithCovarianceStamped() joints = 0 if v>4: #because head, eyes and ears lead to high error for the actor # if v==5 or v == 6: p = Pose() p.position = es_root_cam self.triangulated_root.poses.append(p) gt = joints_gt.poses[alpha_to_gt_joints[k]].position error += (self.get_distance_from_point(gt,es_root_cam)) err_cov.pose.pose.position = es_root_cam err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2 err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2 err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2 err_cov.header.stamp = rospy.Time() err_cov.header.frame_id = 'world' self.triangulated_cov_pub[alpha_to_gt_joints[k]].publish(err_cov) joints+=1 # Publish all estimates self.triangulated_root.header.stamp = rospy.Time() self.triangulated_root.header.frame_id = 'world' self.triangulated_pub.publish(self.triangulated_root) is_in_desired_pos = False error = error/joints if error<=epsilon: # error = 0.4*error/epsilon is_in_desired_pos = True # else: # error = 0.4 return [is_in_desired_pos,error]
def point_tracker(self, pointcloud): # Assume there there is, at most, 1 object (Bonus: get code to work for multiple objects! Hint: use a new kalman filter for each object) if len(pointcloud.points) > 0: point = pointcloud.points[0] measurement = np.matrix([point.x, point.y]).T # note: the shape is critical; this is a matrix, not an array (so that matrix mult. works) else: measurement = None nmeasurements = 2 if measurement is None: # propagate a blank measurement m = np.matrix([np.nan for i in range(2) ]).T xhat, P, K = self.kalman_filter.update( None ) else: # propagate the measurement xhat, P, K = self.kalman_filter.update( measurement ) # run kalman filter ### Publish the results as a simple array float_time = float(pointcloud.header.stamp.secs) + float(pointcloud.header.stamp.nsecs)*1e-9 x = xhat.item(0) # xhat is a matrix, .item() gives you the actual value xdot = xhat.item(1) y = xhat.item(2) ydot = xhat.item(3) p_vector = P.reshape(16).tolist()[0] data = [float_time, x, xdot, y, ydot] data.extend(p_vector) float64msg = Float64MultiArray() float64msg.data = data now = rospy.get_time() self.time_start = now self.pubTrackedObjects_Float64MultiArray.publish( float64msg ) ### ### Publish the results as a ROS type pose (positional information) # see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html pose = PoseWithCovarianceStamped() pose.header = pointcloud.header pose.pose.pose.position.x = x pose.pose.pose.position.y = y pose.pose.pose.position.z = 0 pose.pose.pose.orientation.x = 0 pose.pose.pose.orientation.y = 0 pose.pose.pose.orientation.z = 0 pose.pose.pose.orientation.w = 0 x_x = P[0,0] x_y = P[0,2] y_y = P[2,2] position_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0] position_covariance.extend([0]*24) # position_covariance is the row-major representation of a 6x6 covariance matrix pose.pose.covariance = position_covariance self.pubTrackedObjects_Pose.publish( pose ) ### ### Publish the results as a ROS type twist (velocity information) # see: http://docs.ros.org/jade/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html twist = TwistWithCovarianceStamped() twist.header = pointcloud.header twist.twist.twist.linear.x = xdot twist.twist.twist.linear.y = ydot twist.twist.twist.linear.z = 0 twist.twist.twist.angular.x = 0 twist.twist.twist.angular.y = 0 twist.twist.twist.angular.z = 0 dx_dx = P[1,1] dx_dy = P[1,3] dy_dy = P[3,3] velocity_covariance = [x_x, x_y, 0, 0, 0, 0, x_y, y_y, 0, 0, 0, 0] velocity_covariance.extend([0]*24) # position_covariance is the row-major representation of a 6x6 covariance matrix twist.twist.covariance = velocity_covariance self.pubTrackedObjects_Twist.publish( twist )
def __init__(self): # soundhandle = SoundClient() rospy.init_node('nav_test', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 1.5) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # Pose coordinates are then displayed in the terminal # that was used to launch RViz. locations = OrderedDict(); # ---------------4 POINTS----------------------------------------------------------- # # Locations for bigger simulation map locations['PIT STOP 1'] = Pose(Point(1.6, -2.5, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680)) locations['PIT STOP 2'] = Pose(Point(1.7, 2.5, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975)) locations['PIT STOP 3'] = Pose(Point(-3.3, 2.5, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743)) locations['PIT STOP 4'] = Pose(Point(-3.5, -2.6, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618)) # # Locations for real lab environment (low precision) real1 # locations['PIT STOP 1'] = Pose(Point(0.5, 1, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680)) # locations['PIT STOP 1.5'] = Pose(Point(1.7, 1.8, 0.000), Quaternion(0.000, 0.000, -0.02, 1)) # locations['PIT STOP 2'] = Pose(Point(4.43, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0)) # # locations['SCENIC SPOT 0.5'] = Pose(Point(3.4, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0)) # locations['SCENIC SPOT 1'] = Pose(Point(2.9, 1.08, 0.000), Quaternion(0.000, 0.000, 0.75, 0)) # locations['PIT STOP 3'] = Pose(Point(4.43, 5.15, 0.000), Quaternion(0.000, 0.000, 0.75, 0.6)) # locations['SCENIC SPOT 2'] = Pose(Point(4.69, 4.14, 0.000), Quaternion(0.000, 0.000, 0.99, 0)) # locations['PIT STOP 4'] = Pose(Point(0.47, 5.05, 0.000), Quaternion(0.000, 0.000, 0.75, -3)) # # Locations for real lab environment (HI precision) real4 # # # locations['PIT STOP 1'] = Pose(Point(0.7, -0.85, 0.000), Quaternion(0.000, 0.000, 0, 1)) # # locations['PIT STOP 1.5'] = Pose(Point(0.97, 0.69, 0.000), Quaternion(0.000, 0.000, -0.015, 0.9998)) # locations['PIT STOP 2'] = Pose(Point(4.08, -0.2, 0.000), Quaternion(0.000, 0.000, 0.9998, 0.016)) # # locations['SCENIC SPOT 0.5'] = Pose(Point(2.730, -0.102, 0.000), Quaternion(0.000, 0.000, 0.999, 0.008)) # locations['SCENIC SPOT 1'] = Pose(Point(2.847, -0.081, 0.000), Quaternion(0.000, 0.000, 0.9999, 0.027)) # locations['PIT STOP 3'] = Pose(Point(4.33, 3.85, 0.000), Quaternion(0.000, 0.000, 0.898, -0.4386)) # locations['SCENIC SPOT 2'] = Pose(Point(4.294, 3.0922, 0.000), Quaternion(0.000, 0.000, 0.9987, -0.049)) # # locations['PIT STOP 4'] = Pose(Point(0.2, 4, 0.000), Quaternion(0.000, 0.000, -0.031, 0.999)) # -------------------------------------------------------------------------- # Publisher to manually control the robot (e.g. to stop it) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # Get the initial pose from the user rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): # If we've gone through the current sequence, # start with a new random sequence if i == n_locations: i = 0 # sequence = sample(locations, n_locations) # RANDOMIZE(only keys no value) sequence = locations.keys() # Skip over first location if it is the same as # the last location if sequence[0] == last_location: i = 1 #-------------------------------- rospy.loginfo("ORDER: %s", str(sequence)) # Get the next location in the current sequence location = sequence[i] # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt(pow(locations[location].position.x - locations[last_location].position.x, 2) + pow(locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt(pow(locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow(locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations last_location = location # Increment the counters i += 1 n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) # Allow 5 minutes to get there finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? # running_time = rospy.Time.now() - start_time # running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%") # rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + # " min Distance: " + str(trunc(distance_traveled, 1)) + " m") # soundhandle.playWave('stop.m4a',self.rest_time) rospy.sleep(self.rest_time)
def encoder_interface(): test_pub = rospy.Publisher('test_chatter', String, queue_size=10) pose_pub = rospy.Publisher('test_pose', PoseWithCovarianceStamped, queue_size=10) rospy.init_node('encoder_interface', anonymous=True) rate = rospy.Rate(10) # 10hz ser = serial.Serial( port='/dev/ttyUSB3', baudrate=115200, bytesize=8, parity=serial.PARITY_NONE, stopbits=1, #timeout=2, xonxoff=False, rtscts=False, #writetimeout=2, dsrdtr=False, #interchartimeout=None ) #9600,8,serial.PARITY_NONE,1,2,False,True,2,False,None #ser = serial.Serial( # port='/dev/ttyUSB1', # baudrate=9600, # parity='N', #serial.PARITY_ODD #EVEN # stopbits=1, #serial.STOPBITS_ONE #TWO # bytesize=8, # xonxoff=1, # rtscts=0 #) while not rospy.is_shutdown(): #test_chatter hello_str = "hello world %s" % rospy.get_time() #rospy.loginfo(hello_str) test_pub.publish(hello_str) #test_pose pose = PoseWithCovarianceStamped() pose.header = std_msgs.msg.Header() pose.header.frame_id = "odom" pose.header.stamp = rospy.Time.now() pose.pose.pose.orientation.w = 1 pose.pose.pose.orientation.x = 1 pose.pose.pose.orientation.y = 1 pose.pose.pose.orientation.z = 1 pose.pose.pose.position.x = 1 + random.random() pose.pose.pose.position.y = random.random() pose.pose.pose.position.z = random.random() try: byte = ser.read() #print chr(int(byte)) print repr(byte) except Exception, e: #print "Failed to read: ", e continue pose_pub.publish(pose) rate.sleep()
def cb_ground_truth(self, msg): self.ground_truth = PoseWithCovarianceStamped() #print str(self.ground_truth) self.ground_truth.header.frame_id = "/map" self.ground_truth.pose.pose = msg.pose.pose
def __init__(self): rospy.init_node('nav_test', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 2) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] locations = dict() #定义四个地点的位置 locations['起始点'] = Pose(Point(1.309, 0.919, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['客厅'] = Pose(Point(3.199, 0.513, 0.000), Quaternion(00.000, 0.000, 0.000, 1.000)) locations['厨房'] = Pose(Point(5.581, 0.977, 0.000), Quaternion(0.000, 0.000, 0.752, 0.660)) locations['卧室'] = Pose(Point(6.223, -2.749, 0.000), Quaternion(0.000, 0.000, 0.985, -0.170)) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_loop = 0 #定义控制循环,当为1时停止循环 first = 0 #定义第一次变量 n_locations = len(locations) n_goals = 0 n_successes = 0 i = n_locations distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" self.Dist = 0 self.Asr = "" Loc_1 = 0 Loc_2 = 0 Loc_3 = 0 Name_1 = 0 name_2 = 0 Name = "" Drink_1 = 0 Drink_2 = 0 Drink_3 = 0 Drink = "" # Get the initial pose from the user (how to set) rospy.loginfo( "*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..." ) rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") #dict_keys = ['起始点','厨房','卧室'] dict_keys = ['客厅', '卧室', '厨房'] pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5) #tts pub2 = rospy.Publisher('/voice/xf_asr_topic', Int32, queue_size=5) #asr #pub3 = rospy.Publisher('arm',String,queue_size=5) #arm #pub4 = rospy.Publisher('tracking',Int32,queue_size=5) #物体识别 pub5 = rospy.Publisher('/following', Int32, queue_size=5) #人体跟随 #pub6 = rospy.Publisher('drink_type',String,queue_size=5) #拿的物体类别 pub7 = rospy.Publisher('ros2_wake', Int32, queue_size=5) #ros2opencv2 #pub8 = rospy.Publisher('/voice/xf_nav_follow',String,queue_size=5) #nav_follow ####################################### # Begin the main loop and run through a sequence of locations while n_loop != 1: if first == 0: #如果是第一次 i = 0 location = "起始点" first += 1 # sequence = sample(locations, n_locations) # Skip over first location if it is the same as # the last location #if dict_keys[0] == last_location: # i = 1 ##################################### ####### Modify by XF 2017.4.14 ###### # location = sequence[i] # rospy.loginfo("location= " + str(location)) # location = locations.keys()[i] #location = dict_keys[i] ##################################### # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt( pow( locations[location].position.x - locations[last_location].position.x, 2) + pow( locations[location].position.y - locations[last_location].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt( pow( locations[location].position.x - initial_pose.pose.pose.position.x, 2) + pow( locations[location].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" # Store the last location for distance calculations #last_location = location #rospy.loginfo("test 1 last_location="+str(last_location)) # Increment the counters #i += 1 #rospy.loginfo("test 2 i="+str(i)) n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[location] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(location)) # Start the robot toward the next location self.move_base.send_goal(self.goal) # Allow 6 minutes to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(360)) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.ABORTED: # can not find a plan,give feedback rospy.loginfo("please get out") status_n = "please get out" pub1.publish(status_n) if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) ############ add voice by XF 4.25 ################## The 1st # pub1 = rospy.Publisher('/voice/xf_tts_topic', String, queue_size=5) loc = "主人,我已到" + str(location) pub1.publish(loc) #tts rospy.sleep(5) # rospy.Subscriber('dist',Int32,self.callbackDist) #now robot already adjust correctly # rospy.loginfo("Dist:"+str(self.Dist)) commandA1 = "您好,请问有什么指示?" pub1.publish(commandA1) rospy.sleep(3) #while self.Asr == "": #awake=1 #pub2.publish(awake) #asr #rospy.Subscriber('/voice/xf_asr_follow',String,self.callbackAsr) #获取asr发过来的说话内容 #rospy.sleep(10) #rospy.loginfo("self.Asr:"+str(self.Asr)) #if self.Asr != "": # self.Asr="" #while self.Asr == "": awake = 1 pub2.publish(awake) #asr rospy.Subscriber('/voice/xf_asr_follow', String, self.callbackAsr) #获取asr发过来的说话内容 rospy.sleep(10) rospy.loginfo("self.Asr:" + str(self.Asr)) #if self.Asr != "": # self.Asr="" #rospy.sleep(10) Loc_1 = string.find( self.Asr, '客厅') #find函数,返回所查字符串在整个字符串的起始位置,如果没有,则返回-1 Loc_2 = string.find(self.Asr, '厨房') Loc_3 = string.find(self.Asr, '卧室') #Name_1=string.find(self.Asr,'郭晓萍') #Name_2=string.find(self.Asr,'方璐') # Drink_1=string.find(self.Asr,'可乐') # Drink_2=string.find(self.Asr,'雪碧') # Drink_3=string.find(self.Asr,'橙汁') if Loc_1 != -1: rospy.loginfo("匹配客厅") #dict_keys = [last_location,'客厅'] last_location = location location = "客厅" if Loc_2 != -1: rospy.loginfo("匹配厨房") last_location = location location = "厨房" #dict_keys = [last_location,'卧室'] if Loc_3 != -1: rospy.loginfo("匹配卧室") last_location = location #dict_keys=[last_location,'厨房'] location = "卧室" #if Name_1!=-1: # Name = '郭晓萍' # rospy.loginfo("匹配郭晓萍") #if Name_2!=-1: # Name = '方璐' # rospy.loginfo("匹配方璐") # if Drink_1!=-1: # Drink = '可乐' # if Drink_2!=-1: # Drink = '雪碧' # if Drink_3!=-1: # Drink = '橙汁' # ros2_wake=1 # pub7.publish(ros2_wake) # pub6.publish(Drink) ####################################### self.Asr = "" rospy.sleep(5) commandA2 = "收到指示,请稍等." pub1.publish(commandA2) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)
init_pos.pose.pose.orientation.x, init_pos.pose.pose.orientation.y, init_pos.pose.pose.orientation.z, -init_pos.pose.pose.orientation.w ] q2 = quaternion_from_euler(0, 0, 0) qr = tf.transformations.quaternion_multiply(q2, q1_inv) t.transform.rotation.x = qr[0] t.transform.rotation.y = qr[1] t.transform.rotation.z = qr[2] t.transform.rotation.w = qr[3] br.sendTransform(t) def init_map(init_hedge, init_imu): init_pos.pose.pose.position = init_hedge.pose.pose.position init_pos.pose.pose.orientation = init_imu.orientation if __name__ == "__main__": rospy.init_node("tf2_drift_publisher") init_pos = PoseWithCovarianceStamped() init_hedge = rospy.wait_for_message("/hedge_pose", PoseWithCovarianceStamped) init_imu = rospy.wait_for_message("/imu", Imu) rospy.loginfo("Received Initial Hedge Pose") init_map(init_hedge, init_imu) rospy.spin()
def __init__(self): rospy.on_shutdown(self.cleanup) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait for the action server to become available self.move_base.wait_for_server(rospy.Duration(120)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by the user in RViz initial_pose = PoseWithCovarianceStamped() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Get the initial pose from the user rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Ready to go") rospy.sleep(1) locations = dict() # Location A A_x = 1.0 A_y = 1.0 A_theta = 1.5708 quaternion = quaternion_from_euler(0.0, 0.0, A_theta) locations['A'] = Pose(Point(A_x, A_y, 0.000), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])) self.goal = MoveBaseGoal() rospy.loginfo("Starting navigation test") while not rospy.is_shutdown(): self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Robot will go to point A if start == 1: rospy.loginfo("Going to point A") rospy.sleep(2) self.goal.target_pose.pose = locations['A'] self.move_base.send_goal(self.goal) waiting = self.move_base.wait_for_result(rospy.Duration(300)) if waiting == 1: rospy.loginfo("Reached point A") rospy.sleep(2) rospy.loginfo("Ready to go back") rospy.sleep(2) global start start = 0 # After reached point A, robot will go back to initial position elif start == 0: rospy.loginfo("Going back home") rospy.sleep(2) self.goal.target_pose.pose = self.origin self.move_base.send_goal(self.goal) waiting = self.move_base.wait_for_result(rospy.Duration(300)) if waiting == 1: rospy.loginfo("Reached home") rospy.sleep(2) global start start = 2 rospy.Rate(5).sleep()
def initial_pos_pub(self): # arucoCodeBroadcaster = TransformBroadcaster() # arucoCodeBroadcaster.sendTransform( # (6.56082, 6.63258, 0.0), # (-0.0635783, -0.704243, -0.704243, -0.0635782), # rospy.Time.now(), # "aruco_code", # "map" # ) # this two line is used to stop the while loop # when you using CTRL+C to exit the program signal.signal(signal.SIGINT, quit) signal.signal(signal.SIGTERM, quit) # listener.waitForTransform("/map", "/camera_init", rospy.Time(), rospy.Duration(4.0)) # while not map_camera_trans: # try: # # rospy.sleep(1) # # rospy.loginfo(map_camera_trans) # time_now = rospy.Time.now() # listener.waitForTransform("/map", "/camera_init", time_now, rospy.Duration(4.0)) # # listener.waitForTransform("/map", "/camera_init", rospy.Time(0), rospy.Duration(1.0)) # (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', time_now) # # (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # # map_camera_trans = [0, 0, 0] # # map_camera_rot = [0, 0, 0, 0] # # traceback.print_exc() # rospy.loginfo("stop!!!-----------") # continue map_camera_trans = [] listener = tf.TransformListener() rospy.sleep(0.1) # count = 0 while not map_camera_trans: try: # rospy.loginfo(map_camera_trans) # time_now = rospy.Time.now() # listener.waitForTransform("/map", "/camera_init", time_now, rospy.Duration(5.0)) # listener.waitForTransform("/map", "/camera_init", rospy.Time(0), rospy.Duration(4.0)) # (map_camera_trans, map_camera_rot) = listener.lookupTransform('/map', '/camera_init', time_now) (map_camera_trans, map_camera_rot) = listener.lookupTransform( '/map', '/camera_init', rospy.Time(0)) rospy.loginfo("********Got Trans & Rot********") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): # map_camera_trans = [0, 0, 0] # map_camera_rot = [0, 0, 0, 0] # traceback.print_exc() rospy.loginfo("********NO CODE --OR-- NO TF ********") rospy.sleep(0.1) # rospy.loginfo("stop!!!-----------") continue self.count = self.count + 1 self.trans_total[0] = self.trans_total[ 0] + map_camera_trans[0] / self.NUM_USED_FOR_AVRG self.trans_total[1] = self.trans_total[ 1] + map_camera_trans[1] / self.NUM_USED_FOR_AVRG self.rot_total[ 0] = self.rot_total[0] + map_camera_rot[0] / self.NUM_USED_FOR_AVRG self.rot_total[ 1] = self.rot_total[1] + map_camera_rot[1] / self.NUM_USED_FOR_AVRG self.rot_total[ 2] = self.rot_total[2] + map_camera_rot[2] / self.NUM_USED_FOR_AVRG self.rot_total[ 3] = self.rot_total[3] + map_camera_rot[3] / self.NUM_USED_FOR_AVRG # rospy.loginfo(map_camera_trans[0]) # rospy.loginfo(self.trans_total[0]) if self.count >= self.NUM_USED_FOR_AVRG: rospy.loginfo(self.count) self.trans_total_seq[0] = self.trans_total_seq[ 0] + self.trans_total[0] / self.MAX_SEQ_NEED self.trans_total_seq[1] = self.trans_total_seq[ 1] + self.trans_total[1] / self.MAX_SEQ_NEED self.rot_total_seq[0] = self.rot_total_seq[ 0] + self.rot_total[0] / self.MAX_SEQ_NEED self.rot_total_seq[1] = self.rot_total_seq[ 1] + self.rot_total[1] / self.MAX_SEQ_NEED self.rot_total_seq[2] = self.rot_total_seq[ 2] + self.rot_total[2] / self.MAX_SEQ_NEED self.rot_total_seq[3] = self.rot_total_seq[ 3] + self.rot_total[3] / self.MAX_SEQ_NEED self.trans_total = [0, 0, 0] self.rot_total = [0, 0, 0, 0] self.seq = self.seq + 1 self.count = 0 if self.seq >= self.MAX_SEQ_NEED: # --------------------------------------------------------------------- # The following is going to update the /map->/odom TF frame transform # https://robot-ros.com/robot/35127.html # map_odom_tf = tf.transformations.concatenate_matrices(tf.transformations.translation_matrix( # self.trans_total_seq), tf.transformations.quaternion_matrix(self.rot_total_seq)) # inversed_map_odom_tf = tf.transformations.inverse_matrix(map_odom_tf) # # inversed_trans_map_odom = tf.transformations.translation_from_matrix(inversed_map_odom_tf) # inversed_rot_map_odom = tf.transformations.quaternion_from_matrix(inversed_map_odom_tf) # self.tf_map_to_odom_br_.sendTransform( # self.trans_total_seq, # self.rot_total_seq, # # inversed_trans_map_odom, # # inversed_rot_map_odom, # rospy.Time.now(), # "map", # "odom" # ) # rospy.loginfo("Published /map->/odom TF frame transform!") # rospy.sleep(5) # --------------------------------------------------------------------- start_pos = PoseWithCovarianceStamped() # start_pos = initpose_aruco # filling header with relevant information start_pos.header.frame_id = "map" start_pos.header.seq = self.seq # start_pos.header.stamp = rospy.Time.now() # filling payload with relevant information gathered from subscribing # to initialpose topic published by RVIZ via rostopic echo initialpose start_pos.pose.pose.position.x = self.trans_total_seq[0] # rospy.loginfo(self.trans_total[0]) start_pos.pose.pose.position.y = self.trans_total_seq[1] start_pos.pose.pose.position.z = 0.0 start_pos.pose.pose.orientation.x = self.rot_total_seq[0] start_pos.pose.pose.orientation.y = self.rot_total_seq[1] start_pos.pose.pose.orientation.z = self.rot_total_seq[2] start_pos.pose.pose.orientation.w = self.rot_total_seq[3] # start_pos.pose.pose.position.x=map_camera_trans[0] # start_pos.pose.pose.position.y=map_camera_trans[1] # start_pos.pose.pose.position.z=0.0 # start_pos.pose.pose.orientation.x=map_camera_rot[0] # start_pos.pose.pose.orientation.y=map_camera_rot[1] # start_pos.pose.pose.orientation.z=map_camera_rot[2] # start_pos.pose.pose.orientation.w=map_camera_rot[3] # start_pos.pose.pose.position.x = 5.0 # start_pos.pose.pose.position.y = 10.0 # start_pos.pose.pose.position.z = 0.0 # start_pos.pose.pose.orientation.x = 0.0 # start_pos.pose.pose.orientation.y = 0.0 # start_pos.pose.pose.orientation.z = -0.694837665627 # start_pos.pose.pose.orientation.w = 0.719166613815 # start_pos.pose.covariance[0] = 0.01 # start_pos.pose.covariance[7] = 0.01 # start_pos.pose.covariance[1:7] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # start_pos.pose.covariance[8:34] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, # 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # start_pos.pose.covariance[35] = 0.001 # rospy.loginfo(start_pos) self.initpose_pub.publish(start_pos) rospy.sleep(1) self.initpose_pub.publish(start_pos) # self.trans_total=[0,0,0] # self.rot_total=[0,0,0,0] self.IF_GOT_INIT = True
import numpy as np from nav_msgs.msg import OccupancyGrid import cv2 rospy.init_node('load_wp') tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) trans = tfBuffer.lookup_transform('map', 'world', rospy.Time(0), rospy.Duration(1.0)) map_file = rospy.get_param("/load_wp/map_path") wp_file = rospy.get_param("/load_wp/path_to_save_wp") text_file = open(map_file, "r") lines = text_file.readlines() wp = PoseWithCovariance() wp_transformed_st = PoseWithCovarianceStamped() d = 4000 bw_color = 33 img = np.ones((d, d, 1), np.uint8) * bw_color dx = rospy.get_param("/load_wp/window/x") dy = rospy.get_param("/load_wp/window/y") cx = rospy.get_param("/load_wp/changed_origin/x") cy = rospy.get_param("/load_wp/changed_origin/y") def readMap_cb(data): objectmap = data map_w = data.info.width map_h = data.info.height map_scale = data.info.resolution
#!/usr/bin/env python import rospy import math import numpy as np from geometry_msgs.msg import PoseWithCovarianceStamped from path_planning.msg import comb from tf.transformations import quaternion_from_euler amcl_init_pose = PoseWithCovarianceStamped() longitude_0 = 113.6938667 * math.pi / 180 latitude_0 = 34.7989201 * math.pi / 180 gps_recovery_x = [] gps_recovery_y = [] gps_recovery_flag = 1 def gps_cb(gpsMsg): global gps_recovery_flag, latitude_0, longitude_0, heading_0, pub, amcl_init_pose latitude_tmp = gpsMsg.Latitude longitude_tmp = gpsMsg.Longitude gps_recovery_x.append(latitude_tmp) gps_recovery_y.append(longitude_tmp)
def spin(self): state_message = PoseWithCovarianceStamped() # http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovariance.html => row major list 6 x 6 matrix while not rospy.is_shutdown(): # after receiving the first cmd_vel if len(self.X_list) != 0: state_message.header.stamp = rospy.Time.now() state_message.header.frame_id = "odom_kf" state_message.pose.pose.position.x = self.X_list[ -1] + self.initial_pose.pose.pose.position.x state_message.pose.pose.position.y = 0 state_message.pose.pose.position.z = 0 state_message.pose.pose.orientation.x = 0 state_message.pose.pose.orientation.y = 0 state_message.pose.pose.orientation.z = 0 state_message.pose.pose.orientation.w = 1 state_message.pose.covariance[0] = self.P_list[-1] self.state_publisher.publish(state_message) self.rate.sleep( ) # publish PoseWithCovarianceStamped msg as per designated Hz if rospy.get_time( ) - self.time_record_cmd_now > MSG_INTERVAL_TIME: pose_path_list = [] pose_path_time = [] scalar_X_list = [] scalar_P_list = [] # Task 2 - C: path based on pose and scan for i in range(len(self.X_list)): scalar_X_list.append(np.asscalar(self.X_list[i])) scalar_P_list.append(np.asscalar(self.P_list[i])) # Task 2 - A: path based on pose for k in range(len(self.pose_diff_list)): if k == 0: pose_path_time.append(self.pose_diff_list[k][0]) pose_path_list.append(self.pose_diff_list[k][1]) else: pose_path_time.append(self.pose_diff_list[k][0] + pose_path_time[k - 1]) pose_path_list.append( self.pose_diff_list[k][1] + pose_path_list[k - 1] ) # recursively accumulate the pose path print("finished!") # Data check on screen print("pose_path", pose_path_list, "length", len(pose_path_list)) print("pose_path_time", pose_path_time) print("X_list", scalar_X_list, "length", len(scalar_X_list)) print("X_time", self.time_accumulation_scan_list, "length", len(self.time_accumulation_scan_list)) #print("P_list", scalar_P_list, "length", len(scalar_P_list)) # file save function csv_data_saver(CSV_SAVE_PATH_POSE, pose_path_time, pose_path_list) csv_data_saver(CSV_SAVE_PATH_POSE_ESTIMATE, self.time_accumulation_scan_list, scalar_X_list) rospy.signal_shutdown("finish!")
def fusion(self): print("Here",self.lidar_LOC,self.camc_LOC,self.camg_LOC) if self.lidar_LOC > 0 or self.camc_LOC > 0 or self.camg_LOC > 0: fused_msg = PoseWithCovarianceStamped() COV=fused_msg.pose.covariance h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'base_link' fused_msg.header = h if self.lidar_LOC > 0 and self.camc_LOC > 0 and self.camg_LOC > 0: print("Fuse all sensors.") fused_msg.pose.pose.position.x = (self.lidar_x+self.camc_x+self.camg_x)/3 fused_msg.pose.pose.position.y = (self.lidar_y+self.camc_y+self.camg_y)/3 fused_msg.pose.pose.position.z = self.lidar_LOC+self.camc_LOC+self.camg_LOC COV[0]=self.lidar_cov[0]*self.camc_cov[0]*self.camg_cov[0] COV[1]=self.lidar_cov[1]*self.camc_cov[1]*self.camg_cov[1] COV[6]=self.lidar_cov[2]*self.camc_cov[2]*self.camg_cov[2] COV[7]=self.lidar_cov[3]*self.camc_cov[3]*self.camg_cov[3] fused_msg.pose.covariance=COV elif self.lidar_LOC > 0 and self.camc_LOC > 0: print("Fuse Lidar and color.") fused_msg.pose.pose.position.x = (self.lidar_x+self.camc_x)/2 fused_msg.pose.pose.position.y = (self.lidar_y+self.camc_y)/2 fused_msg.pose.pose.position.z = self.lidar_LOC+self.camc_LOC COV[0]=self.lidar_cov[0]*self.camc_cov[0] COV[1]=self.lidar_cov[1]*self.camc_cov[1] COV[6]=self.lidar_cov[2]*self.camc_cov[2] COV[7]=self.lidar_cov[3]*self.camc_cov[3] fused_msg.pose.covariance=COV elif self.lidar_LOC > 0 and self.camg_LOC > 0: print("Fuse Lidar and geometry.") fused_msg.pose.pose.position.x = (self.lidar_x+self.camg_x)/2 fused_msg.pose.pose.position.y = (self.lidar_y+self.camg_y)/2 fused_msg.pose.pose.position.z = self.lidar_LOC+self.camg_LOC COV[0]=self.lidar_cov[0]*self.camg_cov[0] COV[1]=self.lidar_cov[1]*self.camg_cov[1] COV[6]=self.lidar_cov[2]*self.camg_cov[2] COV[7]=self.lidar_cov[3]*self.camg_cov[3] fused_msg.pose.covariance=COV elif self.camc_LOC > 0 and self.camg_LOC > 0: print("Fuse Color and Geometry.") fused_msg.pose.pose.position.x = (self.camc_x+self.camg_x)/2 fused_msg.pose.pose.position.y = (self.camc_y+self.camg_y)/2 fused_msg.pose.pose.position.z = self.camc_LOC+self.camg_LOC COV[0]=self.camc_cov[0]*self.camg_cov[0] COV[1]=self.camc_cov[1]*self.camg_cov[1] COV[6]=self.camc_cov[2]*self.camg_cov[2] COV[7]=self.camc_cov[3]*self.camg_cov[3] fused_msg.pose.covariance=COV elif self.lidar_LOC > 0: print("Only Lidar.") fused_msg.pose.pose.position.x = self.lidar_x fused_msg.pose.pose.position.y = self.lidar_y fused_msg.pose.pose.position.z = self.lidar_LOC COV[0]=self.lidar_cov[0] COV[1]=self.lidar_cov[1] COV[6]=self.lidar_cov[2] COV[7]=self.lidar_cov[3] fused_msg.pose.covariance=COV elif self.camc_LOC > 0: print("Only Color.") fused_msg.pose.pose.position.x = self.camc_x fused_msg.pose.pose.position.y = self.camc_y fused_msg.pose.pose.position.z = self.camc_LOC COV[0]=self.camc_cov[0] COV[1]=self.camc_cov[1] COV[6]=self.camc_cov[2] COV[7]=self.camc_cov[3] fused_msg.pose.covariance=COV elif self.camg_LOC > 0: print("Only Geometry.") fused_msg.pose.pose.position.x = self.camg_x fused_msg.pose.pose.position.y = self.camg_y fused_msg.pose.pose.position.z = self.camg_LOC COV[0]=self.camg_cov[0] COV[1]=self.camg_cov[1] COV[6]=self.camg_cov[2] COV[7]=self.camg_cov[3] fused_msg.pose.covariance=COV print(fused_msg) self.target_pub.publish(fused_msg)
from nav_msgs.msg import OccupancyGrid from nav_msgs.srv import GetMap from ros_enhsp.srv import ProblemGenerator import rospkg import rospy from std_msgs.msg import String map = OccupancyGrid() def map_callback(msg): global map map = msg robot_pose = PoseWithCovarianceStamped() def pose_callback(msg): global robot_pose robot_pose = msg #### Node function #### def node(): # Initialize node rospy.init_node('problem_interface', anonymous=True) # Subscribers rospy.Subscriber('/map', OccupancyGrid, map_callback) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, pose_callback)
def __init__(self): rospy.init_node('surveil', anonymous=True) rospy.on_shutdown(self.shutdown) # How long in seconds should the robot pause at each location? self.rest_time = rospy.get_param("~rest_time", 5) # Are we running in the fake simulator? self.fake_test = rospy.get_param("~fake_test", False) # Goal state return values goal_states = [ 'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST' ] # Set up the goal locations. Poses are defined in the map frame. # An easy way to find the pose coordinates is to point-and-click # Nav Goals in RViz when running in the simulator. # Pose coordinates are then displayed in the terminal # that was used to launch RViz. loc = ['A', 'B', 'C', 'D'] locations = dict() locations['A'] = Pose(Point(9.5, 10.899, 0.000), Quaternion(0.000, 0.000, 0.594, 0.804)) locations['B'] = Pose(Point(0.699, 3.099, 0.000), Quaternion(0.000, 0.000, 0.908, -0.418)) locations['C'] = Pose(Point(-2.799, -1.800, 0.000), Quaternion(0.000, 0.000, -0.825, 0.564)) locations['D'] = Pose(Point(-7.099, -2.890, 0.000), Quaternion(0.000, 0.000, 0.99, 0.031)) # Publisher to manually control the robot (e.g. to stop it, queue_size=5) self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # A variable to hold the initial pose of the robot to be set by # the user in RViz initial_pose = PoseWithCovarianceStamped() # Variables to keep track of success rate, running time, # and distance traveled n_locations = len(locations) n_goals = 0 n_successes = 0 i = 0 distance_traveled = 0 start_time = rospy.Time.now() running_time = 0 location = "" last_location = "" # Get the initial pose from the user rospy.loginfo( "*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..." ) rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) self.last_location = Pose() rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) # Make sure we have the initial pose while initial_pose.header.stamp == "": rospy.sleep(1) rospy.loginfo("Starting navigation test") # Begin the main loop and run through a sequence of locations while not rospy.is_shutdown(): # Keep track of the distance traveled. # Use updated initial pose if available. if initial_pose.header.stamp == "": distance = sqrt( pow( locations[loc[i]].position.x - locations[loc[il]].position.x, 2) + pow( locations[loc[i]].position.y - locations[loc[il]].position.y, 2)) else: rospy.loginfo("Updating current pose.") distance = sqrt( pow( locations[loc[i]].position.x - initial_pose.pose.pose.position.x, 2) + pow( locations[loc[i]].position.y - initial_pose.pose.pose.position.y, 2)) initial_pose.header.stamp = "" if i == 3: i = 0 elif i != 0: i = i + 1 rospy.loginfo("f**k") n_goals += 1 # Set up the next goal location self.goal = MoveBaseGoal() self.goal.target_pose.pose = locations[loc[i]] self.goal.target_pose.header.frame_id = 'map' self.goal.target_pose.header.stamp = rospy.Time.now() # Let the user know where the robot is going next rospy.loginfo("Going to: " + str(loc[i])) # Start the robot toward the next location self.move_base.send_goal(self.goal) rospy.loginfo("check" + str(loc[i])) # Allow 10 minutes to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(300)) rospy.loginfo("done" + str(loc[i])) # Check for success or failure if not finished_within_time: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("Goal succeeded!") n_successes += 1 distance_traveled += distance rospy.loginfo("State:" + str(state)) else: rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) # How long have we been running? running_time = rospy.Time.now() - start_time running_time = running_time.secs / 60.0 # Print a summary success/failure, distance traveled and time elapsed rospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes / n_goals) + "%") rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m") rospy.sleep(self.rest_time)