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 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 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 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 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 state2pose_sim(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, 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] target_output = PoseWithCovarianceStamped() target_output.pose.pose.position.x = state[2] target_output.pose.pose.position.y = state[3] target_cov = np.zeros((36, )) target_cov[0] = cov[2, 2] target_cov[1] = cov[2, 3] target_cov[6] = cov[3, 2] target_cov[7] = cov[3, 3] target_output.pose.covariance = target_cov.flatten() h = std_msgs.msg.Header() h.stamp = self.t #rospy.Time.now() h.frame_id = 'odom' robot_output.header = h target_output.header = h return target_output, robot_output
def spin(self): rate = rospy.Rate(self.update_freq) while not rospy.is_shutdown(): A = np.diag([1, 1, 1]) u = [0, 0, 0] self.kf.predict(A, u) #if self.measurement_msg: # valid_marker = self.update(self.msg_to_measurement(self.measurement_msg)) # if valid_marker: self.valid_detection_pub.publish(valid_marker) self.last_transform.header.stamp = rospy.Time.now() self.broadcaster.sendTransform(self.last_transform) # determine u by checking if the drone is in motion """ if False and self.cf1_vel: K_vel = 0.5 vx = self.cf1_vel.twist.linear.x * K_vel vy = self.cf1_vel.twist.linear.y * K_vel w = self.cf1_vel.twist.angular.z * K_vel dt = 1.0/self.update_freq A = [1 + abs(vx)*dt, 1 + abs(vy)*dt, 1 + abs(w)*dt] u = [0, 0, 0] self.kf.predict(A, u) elif False and self.cf1_vel: A = np.eye(4) A[0][2] = 1.0/self.update_freq A[1][3] = 1.0/self.update_freq self.kf.predict(A, u) else: A = [1, 1, 1] u = [0, 0, 0] self.kf.predict(A, u) """ if self.cf1_pose: p = PoseWithCovarianceStamped() p.header = self.cf1_pose.header # correct to use cf1/odom as frame_id??? p.pose.pose = self.cf1_pose.pose p.pose.covariance[0] = self.kf.cov[0][0] #p.pose.covariance[1] = self.kf.cov[0][0]*self.kf.cov[1][1] #p.pose.covariance[6] = self.kf.cov[0][0]*self.kf.cov[1][1] p.pose.covariance[7] = self.kf.cov[1][1] p.pose.covariance[-1] = self.kf.cov[2][2] self.cf1_pose_cov_pub.publish(p) rate.sleep()
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 f(msg): if _type == NavSatFix and msg.status.status < 0: # No fix return self.last_msg[index] = rospy.Time.now() if self.active: if index <= self.active_index: self.active_index = index else: dt = (rospy.Time.now() - self.last_msg[self.active_index]).to_sec() if dt > self.timeout: rospy.logwarn( 'Switch to pose source {0} from {1}'.format( self.topics[index], self.topics[self.active_index])) self.active_index = index if index == self.active_index: if _type == PoseWithCovarianceStamped: pose_c = msg elif _type == PoseStamped: pose_c = PoseWithCovarianceStamped() pose_c.header = msg.header pose_c.pose.pose = msg.pose pose_c.pose.covariance = cov elif _type == NavSatFix: pose_c = PoseWithCovarianceStamped() pose_c.header.frame_id = 'utm' pose_c.pose.pose.orientation.w = 1.0 x, y, _, _ = utm.from_latlon(msg.latitude, msg.longitude) z = msg.altitude position = pose_c.pose.pose.position position.x = x position.y = y position.z = z if cov is None: gps_cov = msg.position_covariance else: gps_cov = cov for i in range(3): for j in range(3): pose_c.pose.covariance[6 * i + j] = gps_cov[3 * i + j] else: return # Solve issue with not synchronized publishers pose_c.header.stamp = rospy.Time.now() self.pub.publish(pose_c)
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 fix(self): #self.threadC.acquire() tmp1 = PoseWithCovarianceStamped() tmp1.header = self.odomFilter.header tmp1.pose.pose.position.x = self.world.pose.pose.position.x tmp1.pose.pose.position.y = self.odomFilter.pose.pose.position.y tmp1.pose.pose.position.z = self.odomFilter.pose.pose.position.z tmp1.pose.pose.orientation.x = self.odomFilter.pose.pose.orientation.x tmp1.pose.pose.orientation.y = self.odomFilter.pose.pose.orientation.y tmp1.pose.pose.orientation.z = self.odomFilter.pose.pose.orientation.z tmp1.pose.pose.orientation.w = self.odomFilter.pose.pose.orientation.w tmp1.pose.covariance = self.odomFilter.pose.covariance #tmp1 = msg #tmp1.header.frame_id = "odom" #self.br.sendTransform((0.0,0.0,0.0),(0.0,0.0,0.0,1.0),rospy.Time.now(),"base_link","odom") self.pub.publish(tmp1)
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 correct_strapdown(self, header, x_nav, P_nav, orientation, orientation_cov): msg = PoseWithCovarianceStamped() msg.header = header msg.header.frame_id = "odom" # Transform msg.pose.pose.position.x = x_nav[0, 0] msg.pose.pose.position.y = x_nav[1, 0] msg.pose.pose.position.z = x_nav[2, 0] msg.pose.pose.orientation = orientation new_cov = np.zeros((6, 6)) new_cov[:3, :3] = P_nav[:3, :3] # TODO add full cross correlations new_cov[3:, 3:] = orientation_cov[3:, 3:] msg.pose.covariance = list(new_cov.flatten()) self.intersection_pub.publish(msg)
def path_plan_cb(self, data): # Gets path plan from path_planner node if self.type_ == "Follower": self.start_pos = data.follower.initial self.end_pos = data.follower.goal self.transfer_start_pos = data.follower.line_start self.transfer_end_pos = data.follower.line_end elif self.type_ == "Leader": self.start_pos = data.leader.initial self.end_pos = data.leader.goal self.transfer_start_pos = data.leader.line_start self.transfer_end_pos = data.leader.line_end print("Path plan received") pose_stamped = xytheta_to_pose(*self.start_pos) msg = PoseWithCovarianceStamped() msg.header = pose_stamped.header msg.pose.pose = pose_stamped.pose
def do_transform_pose_with_covariance_stamped(pose, transform): f = transform_to_kdl(transform) * PyKDL.Frame( PyKDL.Rotation.Quaternion( pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w), PyKDL.Vector(pose.pose.pose.position.x, pose.pose.pose.position.y, pose.pose.pose.position.z)) res = PoseWithCovarianceStamped() res.pose.pose.position.x = f.p[0] res.pose.pose.position.y = f.p[1] res.pose.pose.position.z = f.p[2] (res.pose.pose.orientation.x, res.pose.pose.orientation.y, res.pose.pose.orientation.z, res.pose.pose.orientation.w) = f.M.GetQuaternion() res.pose.covariance = pose.pose.covariance res.header = transform.header return res
def utm_cb(msgIn): global wp_num, deg2rad # recieve waypoint converted to odom rospy.loginfo("converted waypoint recieved.") posCovStamped = PoseWithCovarianceStamped() posCovStamped.pose = msgIn.pose posCovStamped.header = msgIn.header # add in des orientation q = quaternion_from_euler(0, 0, gps_waypoints[wp_num][3] * deg2rad) posCovStamped.pose.pose.orientation.x = q[0] posCovStamped.pose.pose.orientation.y = q[1] posCovStamped.pose.pose.orientation.z = q[2] posCovStamped.pose.pose.orientation.w = q[3] # append to global waypoint array waypoints.append(posCovStamped)
def plan_base(self, goal): #find the current base pose from the planning scene robot_state = self.psi.get_robot_state() starting_state = PoseStamped() starting_state.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[ 0] starting_state.pose = robot_state.multi_dof_joint_state.poses[0] goal_robot_state = copy.deepcopy(robot_state) goal_robot_state.multi_dof_joint_state.poses[0] = goal.pose start_markers = vt.robot_marker(robot_state, ns='base_starting_state') goal_markers = vt.robot_marker(goal_robot_state, ns='base_goal_state', g=1, b=0) self.publish(start_markers) self.publish(goal_markers) #plan good_plan = False while not good_plan: try: start_time = time.time() self.base_plan_service(start=starting_state, goal=goal) end_time = time.time() good_plan = True except: rospy.loginfo( 'Having trouble probably with starting state. Sleeping and trying again' ) rospy.sleep(0.2) #it's apparently better just to do the trajectory #than fool with the planning scene goal_cov = PoseWithCovarianceStamped() goal_cov.header = goal.header goal_cov.pose.pose = goal.pose self.pose_pub.publish(goal_cov) self.psi.reset() # self.base.move_to(goal.pose.position.x, goal.pose.position.y, # 2.0*np.arcsin(goal.pose.orientation.z)) return (end_time - start_time)
def publish_pose(self, position=[0, 0, 0], orientation=[0, 0, 0, 1], error=None): msg = PoseStamped() msg.header.frame_id = self.frame_id msg.header.stamp = rospy.Time.now() msg.pose.position = Point(*position) msg.pose.orientation = Quaternion(*orientation) self.pose_pub.publish(msg) self.pose_pub_stat.tick(msg.header.stamp) pmsg = PoseWithCovarianceStamped() pmsg.header = msg.header pmsg.pose.pose = msg.pose pmsg.pose.covariance = self.pose_cov self.pose_cov_pub.publish(pmsg) if error is not None: e = h_error(error) self.error_pub.publish(e)
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 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 spin(self): rate = rospy.Rate(self.update_freq) self.has_transformed = False # to check if initial transform with kalman gain 1 result in good transform while not rospy.is_shutdown(): self.broadcaster.sendTransform(self.last_transform) self.last_transform.header.stamp = rospy.Time.now() A = np.diag([1, 1, 1, 1, 1, 1]) if False and self.cf1_vel: v = np.array([ self.cf1_vel.twist.linear.x, self.cf1_vel.twist.linear.y, self.cf1_vel.twist.linear.z, self.cf1_vel.twist.angular.x, self.cf1_vel.twist.angular.y, self.cf1_vel.twist.angular.z ]) self.kf.predict(A, v) else: self.kf.predict(A) if self.measurement_msg: if not self.has_transformed: map_to_odom = self.update(self.measurement_msg) if map_to_odom: self.last_transform = map_to_odom #self.has_transformed = True #norm = np.linalg.norm(self.kf.cov) #print("Cov norm: {}".format(norm)) self.converged_pub.publish(Bool(self.kf.has_converged())) if self.cf1_pose: p = PoseWithCovarianceStamped() p.header = self.cf1_pose.header # correct to use cf1/odom as frame_id??? p.pose.pose = self.cf1_pose.pose p.pose.covariance[0] = self.kf.cov[0][0] #p.pose.covariance[1] = self.kf.cov[0][1] #p.pose.covariance[6] = self.kf.cov[1][0] p.pose.covariance[7] = self.kf.cov[1][1] p.pose.covariance[-1] = self.kf.cov[5][5] self.cf1_pose_cov_pub.publish(p) rate.sleep()
def callbackPose(poseCV): covariance = [] for i in range(0, 6): covariance.append([]) for j in range(0, 6): covariance[i].append(poseCV.pose.covariance[6 * i + j]) covariance = np.array(covariance) covariance = np.linalg.pinv(covariance) msg = PoseWithCovarianceStamped() msg.header = Header(poseCV.header.seq, poseCV.header.stamp, poseCV.header.frame_id) msg.pose = poseCV.pose data = [] for i in covariance: for j in i: data.append(float(j)) msg.pose.covariance = data ppub.publish(msg) print("(x,y,yaw): (" + str(covariance[0, 0]) + ", " + str(covariance[1, 1]) + ", " + str(covariance[5, 5]) + ")")
def control_gps(self, data_gps): if not self.gps_init: rospy.loginfo('GPS is ON') self.gps_init = True llh = [data_gps.latitude, data_gps.longitude, data_gps.altitude] ned = self.ned.geodetic2ned(llh) msg = PoseWithCovarianceStamped() msg.header = data_gps.header msg.header.frame_id = 'map' msg.pose.pose.position.x = ned[1] msg.pose.pose.position.y = ned[0] msg.pose.pose.position.z = ned[2] msg.pose.pose.orientation.w = 1.0 msg.pose.covariance[0] = data_gps.position_covariance[0] msg.pose.covariance[7] = data_gps.position_covariance[4] msg.pose.covariance[14] = data_gps.position_covariance[8] self.gps_pose_pub.publish(msg)
def getDistToHuman(self, human_pose_in): """Returns distance between human track and robot.""" human_pose_base = None dist = np.inf hpose_out = None hpose = PoseStamped() hpose.header = human_pose_in.header hpose.pose = human_pose_in.pose.pose human_pose_base = self.transformPoseSt(hpose, self.base_frame_id) if (human_pose_base.header.frame_id == self.base_frame_id): dist = np.sqrt((human_pose_base.pose.position.x**2) + (human_pose_base.pose.position.y**2)) hpose_out = PoseWithCovarianceStamped() hpose_out.header = human_pose_base.header hpose_out.pose.pose = human_pose_base.pose hpose_out.pose.covariance = human_pose_in.pose.covariance return (dist, hpose_out)
def spin_publisher(self): r = rospy.Rate(10) while not rospy.is_shutdown(): h = Header(frame_id='map', stamp=rospy.Time.now()) p = PoseWithCovarianceStamped() p.header = h p.pose.pose.position.x = self.x p.pose.pose.position.y = self.y p.pose.pose.orientation = self.q self.pose_pub.publish(p) t = TransformStamped() t.header = h t.child_frame_id = 'base_footprint' t.transform.translation.x = self.x t.transform.translation.y = self.y t.transform.translation.z = 0.0 t.transform.rotation = self.q self.tf_pub.sendTransform(t) r.sleep()
def publish_pose(trfm_msg, map_pub): pub_msg = PoseWithCovarianceStamped() pub_msg.header = trfm_msg.header [ pub_msg.pose.pose.position.x, pub_msg.pose.pose.position.y, pub_msg.pose.pose.position.z ] = [ trfm_msg.transform.translation.x, trfm_msg.transform.translation.y, trfm_msg.transform.translation.z ] [ pub_msg.pose.pose.orientation.x, pub_msg.pose.pose.orientation.y, pub_msg.pose.pose.orientation.z, pub_msg.pose.pose.orientation.w ] = [ trfm_msg.transform.rotation.x, trfm_msg.transform.rotation.y, trfm_msg.transform.rotation.z, trfm_msg.transform.rotation.w ] map_pub.publish(pub_msg)
def spencer_human_tracking_callback(self, msg): with self.data_lock: # after each callback, reset closests self.closest_human_pose = None self.closest_human_twist = None min_dist = np.inf min_i = -1 # TODO: Maybe it's better if we low pass filter these tracks to keep # just humans that have been detected for a minimum period of time #rospy.loginfo("...........................................................") for i, trk_person in enumerate(msg.tracks): # Create the stamped object human_pose = PoseWithCovarianceStamped() # msg contains a header with the frame id for all poses human_pose.header = msg.header human_pose.pose = trk_person.pose # from the list of tracked persons, find the closest ... (dist, human_pose_glob) = self.getDistToHuman(human_pose) #rospy.loginfo("ID: "+str(i)+" Dist: "+str(dist) +" Pose:\n"+str(human_pose)) if dist < min_dist: min_i = i min_dist = dist self.closest_human_pose = human_pose_glob self.closest_human_twist = self.transformTwistWC(trk_person.twist, msg.header, self.global_frame_id) (xh0, yh0, hh0, vh0, wh0) = self.getHumanState() rospy.logdebug_throttle(1, "Node [" + rospy.get_name() + "] " + "Closest human at: Pose ( " + str(xh0) + ", " + str(yh0) + ", " + str(hh0*180.0/np.pi) + " deg), " + "Speed ( " + str(vh0) + " m/sec, " + str(wh0*180.0/np.pi) + " deg/sec) " ) #rospy.loginfo("...........................................................") if (min_dist>0): self.publishQTCdata()
def spin(self): rate = rospy.Rate(self.update_freq) self.has_transformed = False # to check if initial transform with kalman gain 1 result in good transform while not rospy.is_shutdown(): self.broadcaster.sendTransform(self.last_transform) self.last_transform.header.stamp = rospy.Time.now() A = np.diag([1, 1, 1, 1, 1, 1]) if False: #self.cf1_vel: v = np.array([ self.cf1_vel.twist.linear.x, self.cf1_vel.twist.linear.y, self.cf1_vel.twist.linear.z, self.cf1_vel.twist.angular.x, self.cf1_vel.twist.angular.y, self.cf1_vel.twist.angular.z ]) self.kf.predict(A, v) else: self.kf.predict(A) if self.measurement_msg: map_to_odom = self.update(self.measurement_msg) if map_to_odom: self.last_transform = map_to_odom self.converged_pub.publish(Bool(self.kf.has_converged())) if self.cf1_pose: p = PoseWithCovarianceStamped() p.header = self.cf1_pose.header p.pose.pose = self.cf1_pose.pose #self.tf_buf.transform(p, "map") p.pose.covariance[0] = self.kf.cov[0][0] # x p.pose.covariance[1] = self.kf.cov[0][1] # xy p.pose.covariance[6] = self.kf.cov[1][0] # yx p.pose.covariance[7] = self.kf.cov[1][1] # y p.pose.covariance[-1] = self.kf.cov[5][5] # z angle self.cf1_pose_cov_pub.publish(p) rate.sleep()
def send_pose(self, point): # send pose for initing the localization message = PoseWithCovarianceStamped() message.header = self.make_header() pose = PoseWithCovariance() pose.pose = Pose() pose.pose.position = Point() pose.pose.position.x = point[0] pose.pose.position.y = point[1] pose.pose.position.z = 0 pose.pose.orientation = Quaternion() pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 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.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 ] message.pose = pose self.pose_pub.publish(message)
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_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 getDistToHuman(self, human_pose_in): """Returns distance between human track and robot.""" dist = np.inf hpose_out = None if (self.robotPoseSt): hpose = PoseStamped() hpose.header = human_pose_in.header hpose.pose = human_pose_in.pose.pose human_pose_rob = self.transformPoseSt(hpose, self.robotPoseSt.header.frame_id) human_pose_glob = self.transformPoseSt(hpose, self.global_frame_id) if (human_pose_rob) and (human_pose_glob): dist = np.sqrt((human_pose_rob.pose.position.x ** 2) + (human_pose_rob.pose.position.y ** 2)) hpose_out = PoseWithCovarianceStamped() hpose_out.header = human_pose_glob.header hpose_out.pose.pose = human_pose_glob.pose hpose_out.pose.covariance = human_pose_in.pose.covariance return (dist, hpose_out)
def publish_pose(map_frame_id, base_frame_id): global buf while True: try: transform = buf.lookup_transform(map_frame_id, base_frame_id, rospy.Time(0), rospy.Duration(3)) break except tf2_ros.TransformException as e: rospy.logerr(e) continue pose = PoseWithCovarianceStamped() pose.header = transform.header p = pose.pose.pose p.position.x = transform.transform.translation.x p.position.y = transform.transform.translation.y p.position.z = transform.transform.translation.z p.orientation.x = transform.transform.rotation.x p.orientation.y = transform.transform.rotation.y p.orientation.z = transform.transform.rotation.z p.orientation.w = transform.transform.rotation.w initialpose_pub.publish(pose)
def publish_pose(self, particle_avg): # Update pose and TF self.pose_est = np.array(particle_avg) #Generate pose q = tf.transformations.quaternion_from_euler(0, 0, self.pose_est[2]) header = Header(stamp=rospy.Time.now(), frame_id="map") pose = Pose(position=Point(x=self.pose_est[0], y=self.pose_est[1], z=.127), 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([ .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, .075 ])**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 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 publish_uav_target_pose(): if (gun_pose is None): rospy.logwarn_throttle( 10.0, rospy.get_name() + " : Gun pose not yet received") else: target_pose = gun_pose targetOdom = Odometry() # Populating Odometry msg global sequence_number targetOdom.header.frame_id = "odom" targetOdom.header.stamp = rospy.get_rostime() targetOdom.header.seq = sequence_number sequence_number += 1 targetOdom.pose.pose = target_pose uav_target_odom_pub.publish(targetOdom) targetPose = PoseWithCovarianceStamped() targetPose.header = targetOdom.header targetPose.pose.pose = target_pose uav_target_pose_pub.publish(targetPose)
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 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 measSensorLoopTimerCallback(self, timer_msg): # Get time time_stamp_current = rospy.Time.now() # if (self.flag_robot_pose_set == False): return # Computing the measurement # meas_posi = np.zeros((3, ), dtype=float) meas_atti_quat = ars_lib_helpers.Quaternion.zerosQuat() # Position meas_posi[0] = self.robot_posi[0] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_pos['x'])) meas_posi[1] = self.robot_posi[1] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_pos['y'])) meas_posi[2] = self.robot_posi[2] + np.random.normal( loc=0.0, scale=math.sqrt(self.cov_meas_pos['z'])) # Attitude noise_atti_ang = np.zeros((3, ), dtype=float) noise_atti_ang[0] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['x'])) noise_atti_ang[1] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['y'])) noise_atti_ang[2] = np.random.normal(loc=0.0, scale=math.sqrt( self.cov_meas_att['z'])) noise_atti_quat_tf = tf.transformations.quaternion_from_euler( noise_atti_ang[0], noise_atti_ang[1], noise_atti_ang[2], axes='sxyz') noise_atti_quat = np.roll(noise_atti_quat_tf, 1) meas_atti_quat = ars_lib_helpers.Quaternion.quatProd( self.robot_atti_quat, noise_atti_quat) # Covariance meas_cov_posi = np.diag([ self.cov_meas_pos['x'], self.cov_meas_pos['y'], self.cov_meas_pos['z'], self.cov_meas_att['x'], self.cov_meas_att['y'], self.cov_meas_att['z'] ]) # Filling the message # meas_header_msg = Header() meas_robot_pose_msg = Pose() meas_robot_pose_stamp_msg = PoseStamped() meas_robot_pose_cov_msg = PoseWithCovarianceStamped() # meas_header_msg.frame_id = self.robot_frame_id meas_header_msg.stamp = self.robot_pose_timestamp # Position meas_robot_pose_msg.position.x = meas_posi[0] meas_robot_pose_msg.position.y = meas_posi[1] meas_robot_pose_msg.position.z = meas_posi[2] # Attitude meas_robot_pose_msg.orientation.w = meas_atti_quat[0] meas_robot_pose_msg.orientation.x = meas_atti_quat[1] meas_robot_pose_msg.orientation.y = meas_atti_quat[2] meas_robot_pose_msg.orientation.z = meas_atti_quat[3] # meas_robot_pose_stamp_msg.header = meas_header_msg meas_robot_pose_stamp_msg.pose = meas_robot_pose_msg # meas_robot_pose_cov_msg.header = meas_header_msg meas_robot_pose_cov_msg.pose.covariance = meas_cov_posi.reshape( (36, 1)) meas_robot_pose_cov_msg.pose.pose = meas_robot_pose_msg # self.meas_robot_pose_pub.publish(meas_robot_pose_stamp_msg) self.meas_robot_pose_cov_pub.publish(meas_robot_pose_cov_msg) # return
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 )
#!/usr/bin/env python import rospy from ms5837.msg import ms5837_data from std_msgs.msg import Header from geometry_msgs.msg import PoseWithCovarianceStamped pub = rospy.Publisher('ms5837_pose', PoseWithCovarianceStamped, queue_size=3) global zero_value zero_value = 0 ros_msg = PoseWithCovarianceStamped() header = Header() header.frame_id = 'ms5837_pose_data' header.stamp = rospy.Time.now() ros_msg.header = header ros_msg.pose.pose.position.z = 0 ros_msg.pose.covariance[8] = 0.0001349092722 def callback(msg): header.frame_id = 'ms5837_pose_data' header.stamp = rospy.Time.now() ros_msg.header = header ros_msg.pose.pose.position.z = msg.depth - zero_value pub.publish(ros_msg) def zeroing(): # zeroing the message zero_value += ros_msg.pose.pose.position.z
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)
def publish_pose(self, last_ekf_state, pre_update_ekf_state, pre_update_ekf_cov, last_true_position, last_true_theta, current_true_odom): print("Publishing EKF pose! Time: {0}".format( rospy.get_rostime().to_sec())) odom = Odometry() header = odom.header header.seq = self.seq_num self.seq_num += 1 header.stamp = rospy.get_rostime() header.frame_id = 'map' position = odom.pose.pose.position position.x, position.y = self.state[0], self.state[1] orientation = odom.pose.pose.orientation quat = quat_from_rpy_rad(0.0, 0.0, self.state[2]) orientation.x, orientation.y, orientation.z, orientation.w = quat # how the hell do I convert covariance between heading and other values # to covariance between quaternion and other values?? # => don't actually care about covariance between anything other than x, y in my applcation # so just set covariances for quaternion to 0? cov = np.zeros((6, 6)) cov[:3, :3] = self.cov[:3, :3] odom.pose.covariance = list(cov.ravel()) # insert velocity information linear_vel = odom.twist.twist.linear linear_vel.x, linear_vel.y, linear_vel.z = self.state[3], self.state[ 4], 0.0 angular_vel = odom.twist.twist.angular angular_vel.z = self.state[5] # TODO check this is right! # don't bother setting covariance since I don't understand it for angular vel's and I don't actually use it self.odom_publisher.publish(odom) if self.publish_rviz_pose: pose = PoseWithCovarianceStamped() pose.header = header pose.pose = odom.pose self.pose_publisher.publish(pose) # ----publish a SimDataMessage for the sim data collector to deal with--- if last_true_position is None: return msg = SimulationDataMsg() msg.true_odom = current_true_odom p = msg.last_true_pos p.x, p.y, p.z = last_true_position msg.last_true_heading = last_true_theta msg.last_ekf_state = last_ekf_state.tolist() # NOTE: we may have had a camera update here # so we want to write the position before jumping due to camera update! position = msg.ekf_odom.pose.pose.position position.x, position.y = pre_update_ekf_state[0], pre_update_ekf_state[ 1] orientation = msg.ekf_odom.pose.pose.orientation quat = quat_from_rpy_rad(0.0, 0.0, pre_update_ekf_state[2]) orientation.x, orientation.y, orientation.z, orientation.w = quat cov = np.zeros((6, 6)) cov[:3, :3] = pre_update_ekf_cov[:3, :3] msg.ekf_odom.pose.covariance = list(pre_update_ekf_cov.ravel()) msg.ekf_odom.header.stamp = rospy.get_rostime() # insert velocity information linear_vel = msg.ekf_odom.twist.twist.linear linear_vel.x, linear_vel.y, linear_vel.z = pre_update_ekf_state[ 3], pre_update_ekf_state[4], 0.0 angular_vel = msg.ekf_odom.twist.twist.angular angular_vel.z = pre_update_ekf_state[5] # insert camera update if had any if self.last_sensor_position is None: msg.has_camera_update = False else: msg.has_camera_update = True # save the saved values into the message # have to do it this way because # ROS may interrupt/receive new data # at any time it seems... msg_pos = msg.camera_update.position msg_pos.x, msg_pos.y = self.last_sensor_position msg.camera_update.covariance = self.last_sensor_cov.ravel().tolist( ) self.to_sim_data_collector.publish(msg)
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 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 publish_tf(self, pose, stamp=None): """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """ if stamp == None: stamp = rospy.Time.now() # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("map", stamp) odom.child_frame_id = "base_link" odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2]) self.odom_pub.publish(odom) #-------------------------- SAMUEL - added - Publish PoseWithCovariance Msg --------------------------------- if self.PUBLISH_POSE_COVARIANCE: pose_covariance = PoseWithCovarianceStamped() pose_covariance.header = Utils.make_header("map", stamp) pose_covariance.pose.pose.position.x = pose[0] pose_covariance.pose.pose.position.y = pose[1] pose_covariance.pose.pose.orientation = Utils.angle_to_quaternion( pose[2]) # Copy in the Covariance Matrix, Converting from 3-D to 6-D temp_covariance = self.covariance_generator( self.expected_square_pose(), self.square_expected_pose()) for i in range(0, 2): for j in range(0, 2): pose_covariance.pose.covariance[6 * i + j] = temp_covariance[i][j] pose_covariance.pose.covariance[6 * 5 + 5] = temp_covariance[2][2] self.pose_covariance_pub.publish(pose_covariance) # Publishing "map -> laser" TF Transform if self.TF_MODE == 1: self.pub_tf.sendTransform( (pose[0], pose[1], 0), tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp, "laser", "map") # Publishing "map -> base_link" TF Transform elif self.TF_MODE == 2: self.pub_tf.sendTransform( (pose[0], pose[1], 0), tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp, "base_link", "map") # Publishing "map -> odom" TF Transform elif self.TF_MODE == 3: try: # "odom -> base" transform (odom_base_trans, odom_base_rot) = self.tfTL.lookupTransform( "odom", "base_link", rospy.Time(0)) odom_base_trans_mat = tf.transformations.translation_matrix( odom_base_trans) odom_base_rot_mat = tf.transformations.quaternion_matrix( odom_base_rot) odom_base_mat = np.dot(odom_base_trans_mat, odom_base_rot_mat) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): return # "map -> base" transform map_base_trans = [pose[0], pose[1], 0] map_base_rot = tf.transformations.quaternion_from_euler( 0, 0, pose[2]) map_base_trans_mat = tf.transformations.translation_matrix( map_base_trans) map_base_rot_mat = tf.transformations.quaternion_matrix( map_base_rot) map_base_mat = np.dot(map_base_trans_mat, map_base_rot_mat) # "base -> map" transform base_map_mat = tf.transformations.inverse_matrix(map_base_mat) base_map_trans = tf.transformations.translation_from_matrix( base_map_mat) base_map_rot = tf.transformations.quaternion_from_matrix( base_map_mat) # "odom -> map" transform odom_map_mat = np.dot(odom_base_mat, base_map_mat) # "map -> odom" transform map_odom_mat = tf.transformations.inverse_matrix(odom_map_mat) map_odom_trans = tf.transformations.translation_from_matrix( map_odom_mat) map_odom_rot = tf.transformations.quaternion_from_matrix( map_odom_mat) self.pub_tf.sendTransform(map_odom_trans, map_odom_rot, stamp, "odom", "map") else: print "--------------- WARNING : TF_MODE was wrongly selected ---------------" #------------------------------------------------------------------------------------------------------------ return # below this line is disabled """ Our particle filter provides estimates for the "laser" frame since that is where our laser range estimates are measure from. Thus, We want to publish a "map" -> "laser" transform. However, the car's position is measured with respect to the "base_link" frame (it is the root of the TF tree). Thus, we should actually define a "map" -> "base_link" transform as to not break the TF tree. """ # Get map -> laser transform. map_laser_pos = np.array((pose[0], pose[1], 0)) map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2])) # Apply laser -> base_link transform to map -> laser transform # This gives a map -> base_link transform laser_base_link_offset = (0, 0, 0) map_laser_pos -= np.dot( tf.transformations.quaternion_matrix( tf.transformations.unit_vector(map_laser_rotation))[:3, :3], laser_base_link_offset).T # Publish transform self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp, "base_link", "map")