def cb(self, msg): odom = Odometry() odom.header = Header() odom.header = msg.header odom.pose.pose = msg.pose self.pubodom.publish(odom)
def odom_callback(self, msg): odom_rotated = Odometry() odom = msg pose = odom.pose twist = odom.twist #tranform twist into odom frame linear_twist = twist.twist.linear angular_twist = twist.twist.angular twist_rotated = TwistWithCovariance() transformed_linear = self.transform_meas_to_world( tl(linear_twist), odom.child_frame_id, odom.header.frame_id, rospy.Time(0), False) transformed_angular = self.transform_meas_to_world( tl(angular_twist), odom.child_frame_id, odom.header.frame_id, rospy.Time(0), False) twist_rotated.twist.linear = tm(transformed_linear, Vector3) twist_rotated.twist.angular = tm(transformed_angular, Vector3) twist_rotated.covariance = twist.covariance #may need to be transformed odom_rotated.header = odom.header odom_rotated.child_frame_id = odom.child_frame_id odom_rotated.pose = pose odom_rotated.twist = twist_rotated self.rotated_twist_pub.publish(odom_rotated)
def agentCallback(self, data): # msg = self.transform(data) pose = PoseStamped() pose.header = data.header pose.pose = data.pose.pose vector = Vector3Stamped() vector.header = data.header vector.vector = data.twist.twist.linear try: pose = self.tl.transformPose('world', pose) vector = self.tl.transformVector3('world', vector) except: print 'no transform to world frame' return msg = Odometry() msg.header = data.header msg.header.frame_id = 'world' msg.child_frame_id = data.child_frame_id msg.pose.pose = pose.pose msg.twist.twist.linear = vector.vector self.agentPublisher.publish(msg)
def pub_ekf_odom(self, msg): odom = Odometry() odom.header = msg.header odom.child_frame_id = 'base_footprint' odom.pose = msg.pose self.ekf_pub.publish(odom)
def publish_ground_truth(): rospy.init_node('pose_ground_truth') odom_pub = rospy.Publisher('/pose_ground_truth', Odometry, queue_size=10) rospy.wait_for_service('/gazebo/get_model_state') get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) odom = Odometry() header = Header() header.frame_id = 'odom' child_frame = 'base_link' model = GetModelStateRequest() model.model_name = 'rrbot' r = rospy.Rate(20) while not rospy.is_shutdown(): result = get_model_srv(model) odom.pose.pose = result.pose odom.twist.twist = result.twist header.stamp = rospy.Time.now() odom.header = header odom.child_frame_id = child_frame odom_pub.publish(odom) r.sleep()
def odom_transform_2d(self, data, new_frame, trans, rot): # NOTES: only in 2d rotation # also, it removes covariance, etc information odom_new = Odometry() odom_new.header = data.header odom_new.header.frame_id = new_frame odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0] odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1] odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2] # rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot)))) q = data.pose.pose.orientation q_tuple = (q.x, q.y, q.z, q.w,) # Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot)) heading_change = quaternion_to_heading(rot) odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change) odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change) odom_new.twist.twist.linear.z = 0 odom_new.twist.twist.angular.x = 0 odom_new.twist.twist.angular.y = 0 odom_new.twist.twist.angular.z = data.twist.twist.angular.z return odom_new
def publish_pose(self): """ Append dead reckoning from Localization to SLAM estimate to achieve realtime TF. """ pose_msg = PoseWithCovarianceStamped() pose_msg.header.stamp = self.current_frame.time pose_msg.header.frame_id = "map" pose_msg.pose.pose = g2r(self.current_frame.pose3) cov = 1e-4 * np.identity(6, np.float32) # FIXME Use cov in current_frame cov[np.ix_((0, 1, 5), (0, 1, 5))] = self.current_keyframe.transf_cov pose_msg.pose.covariance = cov.ravel().tolist() self.pose_pub.publish(pose_msg) o2m = self.current_frame.pose3.compose( self.current_frame.dr_pose3.inverse()) o2m = g2r(o2m) p = o2m.position q = o2m.orientation self.tf.sendTransform( (p.x, p.y, p.z), [q.x, q.y, q.z, q.w], self.current_frame.time, "odom", "map", ) odom_msg = Odometry() odom_msg.header = pose_msg.header odom_msg.pose.pose = pose_msg.pose.pose odom_msg.child_frame_id = "base_link" odom_msg.twist.twist = self.current_frame.twist self.odom_pub.publish(odom_msg)
def visualize(self): #print 'Visualizing...' self.state_lock.acquire() self.inferred_pose = self.expected_pose() if isinstance(self.inferred_pose, np.ndarray): self.publish_tf(self.inferred_pose) ps = PoseStamped() ps.header = Utils.make_header("map") ps.pose.position.x = self.inferred_pose[0] ps.pose.position.y = self.inferred_pose[1] ps.pose.orientation = Utils.angle_to_quaternion(self.inferred_pose[2]) if(self.pose_pub.get_num_connections() > 0): self.pose_pub.publish(ps) if(self.pub_odom.get_num_connections() > 0): odom = Odometry() odom.header = ps.header odom.pose.pose = ps.pose self.pub_odom.publish(odom) if self.particle_pub.get_num_connections() > 0: if self.particles.shape[0] > self.MAX_VIZ_PARTICLES: # randomly downsample particles proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES, p=self.weights) # proposal_indices = np.random.choice(self.particle_indices, self.MAX_VIZ_PARTICLES) self.publish_particles(self.particles[proposal_indices,:]) else: self.publish_particles(self.particles) if self.pub_laser.get_num_connections() > 0 and isinstance(self.sensor_model.last_laser, LaserScan): self.sensor_model.last_laser.header.frame_id = "/laser" self.sensor_model.last_laser.header.stamp = rospy.Time.now() self.pub_laser.publish(self.sensor_model.last_laser) self.state_lock.release()
def waypoints(): pub = rospy.Publisher('/odom', Odometry, queue_size=1) rospy.init_node('odomer', anonymous=True) rate = rospy.Rate(1) # 10hz odom = Odometry() covarian = [ 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0 ] odom.child_frame_id = "odom" odom.pose.pose.position.x = 1.0 odom.pose.pose.position.y = 0.0 odom.pose.pose.orientation.x = 0.0 odom.pose.pose.orientation.y = 0.0 odom.pose.pose.orientation.z = 0.0 odom.pose.pose.orientation.w = 1.0 odom.pose.covariance = covarian h = std_msgs.msg.Header() h.stamp = rospy.Time.now() odom.header = h odom.twist.twist.linear.x = 0.0 odom.twist.twist.linear.y = 0.0 odom.twist.twist.linear.z = 0.0 odom.twist.twist.angular.x = 0.0 odom.twist.twist.angular.y = 0.0 odom.twist.twist.angular.z = 0.0 odom.twist.covariance = covarian while not rospy.is_shutdown(): pub.publish(odom) rate.sleep()
def process(bag_file): bag_path, bag_name = os.path.split(bag_file) bag_out = os.path.join(bag_path, os.path.splitext(bag_name)[0] + "_p.bag") print "Opening bag ", bag_file, "." bag_in = rosbag.Bag(bag_file, 'r') num_messages = bag_in.get_message_count() print "Done. Num messages: ", num_messages, "." bag_out = rosbag.Bag(bag_out, 'w') i = 0 for topic, msg, t in bag_in.read_messages(): if topic == '/pose_imu': m = Odometry() m.header = msg.header m.child_frame_id = "IMU/GPS" m.pose.pose = msg.pose msg = m bag_out.write(topic, msg, t) i += 1 if i % 1000 == 0: print "Processed ", i, "/", num_messages, " messages." bag_out.close()
def main(model_name): rospy.init_node('odom_pub') print(model_name) odom_pub = rospy.Publisher("/position_" + model_name, Odometry, queue_size=10) rospy.wait_for_service('/gazebo/get_model_state') get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) odom = Odometry() header = Header() header.frame_id = '/odom' model = GetModelStateRequest() model.model_name = model_name r = rospy.Rate(100) while not rospy.is_shutdown(): result = get_model_srv(model) odom.pose.pose = result.pose odom.twist.twist = result.twist header.stamp = rospy.Time.now() odom.header = header odom_pub.publish(odom) print(model_name) r.sleep()
def publish(self, x, y): msg = Odometry() msg.header = Header() msg.header.frame_id = 'target_pose' msg.pose.pose.position.x = x msg.pose.pose.position.y = y self.target_pose_pub.publish(msg)
def callback_state(data): global state state = Pose2D() state.x = data.x state.y = data.y state.theta = data.yaw msg = OdometryState() msg_ros = Odometry() msg.header = data.header msg_ros.header = data.header msg.state.pose.pose.position.x = data.x msg.state.pose.pose.position.y = data.y [x, y, z, w] = quaternion_from_euler(state.theta, 0, 0) msg.state.pose.pose.orientation.x = x msg.state.pose.pose.orientation.y = y msg.state.pose.pose.orientation.z = z msg.state.pose.pose.orientation.w = w msg_ros.pose.pose.position.x = data.x msg_ros.pose.pose.position.y = data.y [x, y, z, w] = quaternion_from_euler(state.theta, 0, 0) msg_ros.pose.pose.orientation.x = x msg_ros.pose.pose.orientation.y = y msg_ros.pose.pose.orientation.z = z msg_ros.pose.pose.orientation.w = w v = np.array([data.vx, data.vy]) msg.velocity = np.sqrt(np.sum(np.square(v))) pub_state.publish(msg) pub_state_ros.publish(msg_ros)
def puhlish_odom_data_to_f110(self): '''publish odometry data ''' loop_rate = rospy.Rate(self.PUBLISH_RATE) while not rospy.is_shutdown(): if self.car_state is not None: current_state = self.car_state #localized state posedata = PoseStamped() posedata.header = self.create_header('map') posedata.pose.position.x = current_state.x posedata.pose.position.y = current_state.y quat_data = quaternion_from_euler(0, 0, current_state.yaw) posedata.pose.orientation.z = quat_data[2] posedata.pose.orientation.w = quat_data[3] self.pose_pub.publish(posedata) #odometry message cmd = Odometry() cmd.header = self.create_header('map') cmd.child_frame_id = 'base_link' cmd.twist.twist.linear.x = np.linalg.norm( np.array([current_state.vx, current_state.vy])) self.odom_pub.publish(cmd) loop_rate.sleep()
def computeOdom(self): while True: self.computeSpeed() xdot = (1 / 2) * math.cos(self.theta) * self.speedRight + ( 1 / 2) * math.cos(self.theta) * self.speedLeft ydot = (1 / 2) * math.sin(self.theta) * self.speedRight + ( 1 / 2) * math.sin(self.theta) * self.speedLeft thetadot = (1.0 / WHEEL_BASE) * self.speedRight - ( 1.0 / WHEEL_BASE) * self.speedLeft self.x = self.x + 0.05 * xdot self.y = self.y + 0.05 * ydot self.theta = self.theta + 0.05 * thetadot q = euler2quat(0.0, 0.0, self.theta) odom = Odometry() h = Header() h.stamp = rospy.Time.now() h.frame_id = 'odom' odom.header = h odom.child_frame_id = 'base_link' odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.orientation.x = q[1] odom.pose.pose.orientation.y = q[2] odom.pose.pose.orientation.z = q[3] odom.pose.pose.orientation.w = q[0] self.odomPub.publish(odom) time.sleep(0.05)
def pose_cb(data): global path global count global path_len, x_prev, y_prev, z_prev x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z dist = ((x - x_prev)**2 + (y - y_prev)**2 + (z - z_prev)**2)**0.5 #Temporary value of 0.5 used for eliminating jumps. CHANGE THIS LATER if dist < 0.5: path_len = path_len + dist else: path_len = 0 x_prev, y_prev, z_prev = x, y, z #print("Path Length = "+str(path_len)) path.header = data.header path.poses.append(data) path_pub.publish(path) count = (count + 1) % 20 if not count: odom = Odometry() odom.header = data.header odom.pose.pose = data.pose odom_pub.publish(odom) print("Path Length = " + str(path_len))
def pub_ekf_odom(self, msg): odom = Odometry() odom.header = msg.header odom.child_frame_id = 'angelbot_base' odom.pose = msg.pose self.ekf_pub.publish(odom)
def vioCallback(self, uav_odom): tf_odom = Odometry() tf_odom.header = uav_odom.header x = uav_odom.pose.pose.position.x y = uav_odom.pose.pose.position.y z = uav_odom.pose.pose.position.z pos = (x, y, z) x = uav_odom.pose.pose.orientation.x y = uav_odom.pose.pose.orientation.y z = uav_odom.pose.pose.orientation.z w = uav_odom.pose.pose.orientation.w qua = (x, y, z, w) trans = tf.transformations.translation_matrix(pos) rot = tf.transformations.quaternion_matrix(qua) uav_pose_r = np.matmul(trans, rot) # uav's pose in robot coordinates vio_pose_r = np.matmul(self.T_robot2vio, uav_pose_r) # vio's pose in robot coordinates vio_pose_c = np.matmul(vio_pose_r, self.T_vio2robot) tf_pos = tf.transformations.translation_from_matrix(vio_pose_c) tf_qua = tf.transformations.quaternion_from_matrix(vio_pose_c) tf_odom.pose.pose.position.x = tf_pos[0] tf_odom.pose.pose.position.y = tf_pos[1] tf_odom.pose.pose.position.z = tf_pos[2] tf_odom.pose.pose.orientation.x = tf_qua[0] tf_odom.pose.pose.orientation.y = tf_qua[1] tf_odom.pose.pose.orientation.z = tf_qua[2] tf_odom.pose.pose.orientation.w = tf_qua[3] self.pub_vio.publish(tf_odom)
def start(self): rospy.init_node(self.vehicle_type + '_' + self.vehicle_id + "_communication") rate = rospy.Rate(100) ''' main ROS thread ''' while not rospy.is_shutdown(): self.target_motion_pub.publish(self.target_motion) try: response = self.gazeboModelstate( self.vehicle_type + '_' + self.vehicle_id, 'ground_plane') except rospy.ServiceException, e: print "Gazebo model state service call failed: %s" % e odom = Odometry() odom.header = response.header odom.pose.pose = response.pose odom.twist.twist = response.twist self.odom_groundtruth_pub.publish(odom) if (self.flight_mode is "LAND") and (self.local_pose.pose.position.z < 0.15): if (self.disarm()): self.flight_mode = "DISARMED" rate.sleep()
def pub_odom(self, msg): odom = Odometry() odom.header = msg.header odom.child_frame_id = self.base_frame odom.pose = msg.pose self.odom_pub.publish(odom)
def pose_update_cb(pose_data): """ Publishes odometry information :param pose_data: geometry_msgs/PoseWithCovarianceStamped, current pose data from hector_mapping package. :return: """ # Broadcast transform base_link -> odom odom_broadcaster.sendTransform( (pose_data.pose.pose.position.x, pose_data.pose.pose.position.y, pose_data.pose.pose.position.z), (pose_data.pose.pose.orientation.x, pose_data.pose.pose.orientation.y, pose_data.pose.pose.orientation.z, pose_data.pose.pose.orientation.w), pose_data.header.stamp, "base_link", "odom") # Next, we'll publish the odometry message over ROS odom = Odometry() odom.header = pose_data.header odom.header.frame_id = "odom" # Set the position odom.pose = pose_data.pose # Set the velocity odom.child_frame_id = "base_link" odom.twist.twist = Twist(Vector3(vel_x, 0, 0), Vector3(0, 0, vel_yaw)) # Publish the message odom_pub.publish(odom)
def callback_odom(data): # get current transorm if tlisten.frameExists(_base_frame_name) and tlisten.frameExists( _map_frame_name): _t = tlisten.getLatestCommonTime(_base_frame_name, _map_frame_name) try: position, quaternion = tlisten.lookupTransform( _map_frame_name, _base_frame_name, rospy.Time()) except tf.ExtrapolationException as e: rospy.logwarn('Extrapolation Warning') return None # publish odom _msg = Odometry() _msg.header = data.header _msg.header.frame_id = _map_frame_name _msg.child_frame_id = _base_frame_name _msg.pose.pose.orientation.x = quaternion[0] _msg.pose.pose.orientation.y = quaternion[1] _msg.pose.pose.orientation.z = quaternion[2] _msg.pose.pose.orientation.w = quaternion[3] _msg.pose.pose.position.x = position[0] _msg.pose.pose.position.y = position[1] _msg.pose.pose.position.z = position[2] _msg.twist.twist.linear.x = data.twist.twist.linear.x _msg.twist.twist.angular.z = data.twist.twist.linear.x pub.publish(_msg) else: rospy.logwarn('Frame does not exist : %s, %s', _base_frame_name, _map_frame_name)
def pose_callback(self, pose_msg): ''' Uses a pose message to generate an odometry and state message. :param pose_msg: (geometry_msgs/PoseStamped) pose message ''' new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map", pose_msg.header) if not self.use_amcl: pose_msg = new_pose_msg self.new_pose_pub.publish( pose_msg ) # if using AMCL, this will be the same as the original pose message if trans and rot: # if not getting tf, trans and rot will be None footprint = PolygonStamped() footprint.header = pose_msg.header # has same frame_id (map) and time stamp loomo_points = np.array([[0.16, -0.31], [0.16, 0.31], [-0.16, 0.31], [-0.16, -0.31]]) roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot) rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]]) polygon = Polygon( points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T]) footprint.polygon = polygon self.footprint_pub.publish(footprint) odom_msg = Odometry() odom_msg.pose.pose = pose_msg.pose odom_msg.header = pose_msg.header self.odom_pub.publish(odom_msg) state_msg = State() state_msg.header = pose_msg.header state_msg.state_stamp = pose_msg.header.stamp state_msg.pos.x = pose_msg.pose.position.x state_msg.pos.y = pose_msg.pose.position.y state_msg.pos.z = pose_msg.pose.position.z state_msg.quat.x = pose_msg.pose.orientation.x state_msg.quat.y = pose_msg.pose.orientation.y state_msg.quat.z = pose_msg.pose.orientation.z state_msg.quat.w = pose_msg.pose.orientation.w if self.last_state_time and self.last_state: dt = pose_msg.header.stamp.nsecs - self.last_state_time state_msg.vel.x = (state_msg.pos.x - self.last_state.pos.x) / (float(dt) / 10**9) state_msg.vel.y = (state_msg.pos.y - self.last_state.pos.y) / (float(dt) / 10**9) state_msg.vel.z = 0 # ground robot not traveling in z direction self.last_state_time = pose_msg.header.stamp.nsecs self.last_state = state_msg self.state_pub.publish(state_msg)
def pub_ekf_odom(self, msg): odom = Odometry() odom.header = msg.header odom.header.frame_id = '/odom' odom.child_frame_id = 'base_link' odom.pose = msg.pose self.ekf_pub.publish(odom)
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() # this may cause issues with the TF tree. If so, see the below code. # self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), # stamp , "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("/map", stamp) 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) # 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.265, 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 map_laser_pos[0] -= self.position[0] map_laser_pos[1] -= self.position[1] orientation_list = [ self.odom_orientation.x, self.odom_orientation.y, self.odom_orientation.z, self.odom_orientation.w ] (roll, pitch, odom_base_link_yaw ) = tf.transformations.euler_from_quaternion(orientation_list) map_odom_yaw = pose[2] - odom_base_link_yaw map_odom_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, map_odom_yaw)) # Publish transform self.pub_tf.sendTransform(map_laser_pos, map_odom_rotation, stamp, "/odom", "/map")
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() # this may cause issues with the TF tree. If so, see the below code. # self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), # stamp , "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("/map", stamp) odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = Utils.yaw_to_quaternion(pose[2]) self.odom_pub.publish(odom) # 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). And "odom" to "base_link" transform is published by vesc_to_odom node. Thus, we should actually define a "map" -> "odom" 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.06, 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") # Assuming only translational offset, no rotation base_link_laser_delta = 0.06 map_base_link_pos_x = pose[0] - base_link_laser_delta * math.cos( pose[2]) map_base_link_pos_y = pose[1] - base_link_laser_delta * math.sin( pose[2]) # This assumes the map and odom frames are initially aligned map_odom_delta = np.array( (map_base_link_pos_x, map_base_link_pos_y, 0)) map_odom_orientation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2])) self.pub_tf.sendTransform(map_odom_delta, map_odom_orientation, stamp, "/base_link", "/map")
def callback(self, data): lat = data.latitude lon = data.longitude alt = data.altitude e, n, self.zone_number, self.zone_letter = utm.from_latlon(lat, lon) if not self.start_pos_was_set: self.start_e = e self.start_n = n """ if self.offset: self.start_e -= self.offset.translation.x self.start_n -= self.offset.translation.y """ if self.offset: self.start_e -= 0.4 self.start_n -= 0.4 self.start_pos_was_set = True self.start_alt = data.altitude position = Point() position.x = e - self.start_e position.y = n - self.start_n position.z = 0.0 erel = e - self.start_e # relative position to start position in meters nrel = n - self.start_n alt_rel = alt - self.start_alt # empty as the gps data doesnt include orientation orientation = Quaternion() orientation.w = 1.0 mypose = Pose() mypose.orientation = orientation mypose.position = position if self.fix_covariance: covariance = np.array([1.5, 0, 0, 0, 1.5, 0, 0, 0, 3]) else: covariance = data.position_covariance covariance = np.reshape(covariance, (3, 3)) cov6x6 = np.zeros((6, 6)) cov6x6[:3, :3] = covariance cov6x6_flat = np.reshape(cov6x6, (36, )).astype(np.float64) pose_with_cov = PoseWithCovariance() pose_with_cov.covariance = cov6x6_flat pose_with_cov.pose = mypose odom = Odometry() odom.pose = pose_with_cov odom.header = data.header odom.header.stamp = data.header.stamp odom.header.stamp = rospy.Time.now() odom.header.frame_id = "map" # odom.twist stays empty as gps doesn't have this data self.publisher.publish(odom)
def callback(pose): # cov_east = navsatfix.position_covariance[0] # cov_north = navsatfix.position_covariance[4] # if cov_east>cov_thresh or cov_north>cov_thresh: # return odom = Odometry() odom.pose.pose = pose.pose odom.header = pose.header pub.publish(odom)
def transition(self, state, twist, dt): desired = state[1] current = state[0] # do something n = Odometry() n.header = current.header n.twist.twist = self.update_twist(current.twist.twist, twist) n.pose.pose = self.update_pose(current.pose.pose, current.twist.twist, n.twist.twist, dt) return (n, desired, )
def _on_receive_twist(self, twist_stamped_msg): """Receive TwistStamped message from simulation and assign to odom""" odom = Odometry() odom.twist.twist = twist_stamped_msg.twist odom.header = twist_stamped_msg.header odom.header.frame_id = 'odom' odom.child_frame_id = "dvl_link" odom.header.stamp.secs = rospy.get_rostime().secs odom.header.stamp.nsecs = rospy.get_rostime().nsecs self.odom_pub.publish(odom)
def publish_pose_estimate(self, pose): """ Converts from pose estimate to odometry messsage """ odom = Odometry() odom.header = make_header("map") odom.pose.pose.position.x = pose[0] odom.pose.pose.position.y = pose[1] odom.pose.pose.orientation = angle_to_quaternion(pose[2]) self.odom_pub.publish(odom)
def handle_img(self, msg): starttime = time.time() image_np = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8") if image_np.shape[0] != self.intrinsic.shape[0] or image_np.shape[ 1] != self.intrinsic.shape[1]: print( 'The intrinsic parameter does not match the image parameter!') return if self.last_img is not None: pose_msg = PoseStamped() pose_msg.header.stamp = msg.header.stamp pose_msg.header.frame_id = 'map' sample = { 'img1': self.last_img, 'img2': image_np, 'intrinsic': self.intrinsic } sample = self.transform(sample) sample['img1'] = sample['img1'][None] # increase the dimension sample['img2'] = sample['img2'][None] sample['intrinsic'] = sample['intrinsic'][None] motion, _ = self.tartanvo.test_batch(sample) motion = motion[0] # adjust the scale if available # if self.scale!=1: # trans = motion[:3] # trans = trans / np.linalg.norm(trans) * self.scale # motion[:3] = trans print(self.scale) motion_mat = se2SE(motion) self.pose = self.pose * motion_mat quat = SO2quat(self.pose[0:3, 0:3]) pose_msg.pose.position.x = self.scale * self.pose[0, 3] pose_msg.pose.position.y = self.scale * self.pose[1, 3] pose_msg.pose.position.z = self.scale * self.pose[2, 3] pose_msg.pose.orientation.x = quat[0] pose_msg.pose.orientation.y = quat[1] pose_msg.pose.orientation.z = quat[2] pose_msg.pose.orientation.w = quat[3] self.pose_pub.publish(pose_msg) odom_msg = Odometry() odom_msg.header = pose_msg.header odom_msg.pose.pose = pose_msg.pose self.odom_pub.publish(odom_msg) self.last_img = image_np.copy() print(" call back time: {}:".format(time.time() - starttime))
def callbackOdom(msg): global have_first global first_odom if not have_first: first_odom = msg have_first = True print "first_odom: " + str(first_odom.pose.x) odom_msg = Odometry() odom_msg.header = msg.header odom_msg.header.frame_id = 'odom' odom_msg.child_frame_id = msg.child_frame_id odom_msg.child_frame_id = 'base_link' odom_msg.pose.pose.position.x = msg.pose.x - first_odom.pose.x odom_msg.pose.pose.position.y = msg.pose.y - first_odom.pose.y # Wheel radius. # TODO(gbrooks): Parameterize. odom_msg.pose.pose.position.z = 0.127 q = tf.transformations.quaternion_from_euler( 0, 0, msg.pose.theta - first_odom.pose.theta) odom_msg.pose.pose.orientation.x = q[0] odom_msg.pose.pose.orientation.y = q[1] odom_msg.pose.pose.orientation.z = q[2] odom_msg.pose.pose.orientation.w = q[3] odom_msg.pose.covariance = [4, 0, 0, 0, 0, 0, \ 0, 4, 0, 0, 0, 0, \ 0, 0, 4, 0, 0, 0, \ 0, 0, 0, 0.1, 0, 0, \ 0, 0, 0, 0, 0.1, 0, \ 0, 0, 0, 0, 0, 0.1] odom_msg.twist.twist.linear.x = msg.twist.vx odom_msg.twist.twist.linear.y = msg.twist.vy odom_msg.twist.twist.linear.z = 0 odom_msg.twist.twist.angular.x = 0 odom_msg.twist.twist.angular.y = 0 odom_msg.twist.twist.angular.z = msg.twist.vtheta odom_msg.twist.covariance = [0.1, 0, 0, 0, 0, 0, \ 0, 0.1, 0, 0, 0, 0, \ 0, 0, 0.1, 0, 0, 0, \ 0, 0, 0, 0.03, 0, 0, \ 0, 0, 0, 0, 0.03, 0, \ 0, 0, 0, 0, 0, 0.03] odom_pub.publish(odom_msg) odom_br = tf.TransformBroadcaster() odom_br.sendTransform( (odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0), tf.transformations.quaternion_from_euler( 0, 0, msg.pose.theta - first_odom.pose.theta), rospy.Time.now(), "base_link_odom_wheel", "odom")
def __init__(self, rand_size): self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=2) self.odom = None self.carrot_sub = rospy.Subscriber("/lqrrt/ref", Odometry, self.set_odom) fprint("Shaking hands and taking names.") rospy.sleep(1) # We need to publish an inital odom message for lqrrt start_ori = trns.quaternion_from_euler(0, 0, np.random.normal() * 3.14) start_pos = np.append(np.random.uniform(rand_size, size=(2)), 1) start_pose = navigator_tools.numpy_quat_pair_to_pose(start_pos, start_ori) start_odom = Odometry() start_odom.header = navigator_tools.make_header(frame='enu') start_odom.child_frame_id = 'base_link' start_odom.pose.pose = start_pose self.odom_pub.publish(start_odom)
def default(self, ci='unused'): odometry = Odometry() odometry.header = self.get_ros_header() odometry.child_frame_id = self.child_frame_id # fill pose and twist odometry.pose.pose = self.get_pose() odometry.twist.twist = self.get_twist() self.publish(odometry) # send current odometry transform self.sendTransform(self.get_position(), self.get_orientation(), odometry.header.stamp, odometry.child_frame_id, odometry.header.frame_id)
def unpackOdom(self, data): odom_msg = Odometry() # Unpack Header odom_msg.header = self.unpackOdomHeader(data[0:20]) # Unpack Child_frame_id # Is constant and "base_link" cfid = unpack("xxxxccccccccc", data[20:33]) # odom_msg.child_frame_id = (cfid[0] + cfid[1] + cfid[2] + # cfid[3] + cfid[4] + cfid[5] + cfid[6] + cfid[7] + cfid[8]) odom_msg.child_frame_id = "base_footprint" # Unpack Pose pose = self.bytestr_to_array(data[33:89]) odom_msg.pose.pose.position.x = pose[0] odom_msg.pose.pose.position.y = pose[1] odom_msg.pose.pose.position.z = pose[2] odom_msg.pose.pose.orientation.x = pose[3] odom_msg.pose.pose.orientation.y = pose[4] odom_msg.pose.pose.orientation.z = pose[5] odom_msg.pose.pose.orientation.w = pose[6] # Skip Unpack Pose Covariance of Pose # The values are all zeros since we did not know how to properly # calculate them. We are not properly sending the correct covarinace # maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince # we will skip 4+ (9*8) = 76 bytes odom_msg.pose.covariance = covariance # Unpack Twist twist = self.bytestr_to_array(data[165:213]) odom_msg.twist.twist.linear.x = twist[0] odom_msg.twist.twist.linear.y = twist[1] odom_msg.twist.twist.linear.z = twist[2] odom_msg.twist.twist.angular.x = twist[3] odom_msg.twist.twist.angular.y = twist[4] odom_msg.twist.twist.angular.z = twist[5] # Skip Unpack Twist Covariance of Pose # The values are all zeros since we did not know how to properly # calculate them. We are not properly sending the correct covarinace # maxtrix. Ours is 3x3 where it should be 6x6. To skip the covaraince # we will skip 4+ (9*8) = 76 bytes odom_msg.twist.covariance = covariance return odom_msg
def subCB(msg_in): global pub msg_out = Odometry() msg_out.header = msg_in.header msg_out.header.frame_id = "Pioneer3AT/odom" msg_out.child_frame_id = "imu" cart = LLA2Naive(msg_in.LLA.x, msg_in.LLA.y, msg_in.LLA.z) msg_out.pose.pose.position.x = cart[0] msg_out.pose.pose.position.y = cart[1] msg_out.pose.pose.position.z = cart[2] q = tf.transformations.quaternion_from_euler(pi * msg_in.RPY.x / 180.0, pi * msg_in.RPY.y / 180.0, pi * msg_in.RPY.z / -180.0 ) msg_out.pose.pose.orientation = Quaternion(*q) pub.publish(msg_out)
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() # this may cause issues with the TF tree. If so, see the below code. self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), stamp , "/laser", "/map") # also publish odometry to facilitate getting the localization pose if self.PUBLISH_ODOM: odom = Odometry() odom.header = Utils.make_header("/map", stamp) 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) 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.265, 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")
def main(): parser = argparse.ArgumentParser(description="Parse the images.txt file to " "create a rosbag with several " "PoseStamped messages.") parser.add_argument('-i', '--input', help='file with poses (images.txt)', metavar='poses_filename', default='./images.txt', type=str, required=True ) parser.add_argument('-o', '--output', help='output bag file (with location)', metavar='bag_filename', type=str, required=True ) parser.add_argument('-y', help='if images_adjusted.txt is found in the same folder' 'as the supplied filename, then use it without' 'asking', metavar='True/False', type=bool, default=False, choices=[True, False] ) arguments = parser.parse_args() images_folder, _ = os.path.split(arguments.input) alt_file = os.path.join(images_folder, 'images_adjusted.txt') if os.path.exists(alt_file): if arguments.y: images_file = open(alt_file) else: reply = None while reply not in ['y', 'n']: print("The images_adjusted.txt file is in the folder {}, do you" " want to use that instead? [y/n]".format(images_folder)) reply = raw_input() if reply == 'y': images_file = open(alt_file) else: images_file = open(arguments.input) else: images_file = open(arguments.input) print("Processing data from {}...".format(images_file.name)) with rosbag.Bag(arguments.output, 'w') as outputbag: for lineno, line in enumerate(images_file): #lines must be of the form: #image_basename,t_android,t_movidius,r11,r12,r12,t1,r21,r22,r23,t2,r31,r32,r33,t3 tokens = line.strip('\n').split(',') if (len(tokens)) != 15: print("Line {0} appears to be wrong: \"{1}\" ".format(lineno, tokens)) continue #first elements before transformation image_name = tokens[0] ts1 = float(tokens[1]) ts2 = float(tokens[2]) #getting the quaterion out the rotation matrix rotation_matrix = np.array(map(float, tokens[3:])).reshape(3, 4) rotation_matrix = np.vstack((rotation_matrix, [0, 0, 0, 1])) quaternion = transformations.quaternion_from_matrix(rotation_matrix) #creating the pose p = PoseStamped() p.header.frame_id = "/tango/world" p.header.stamp = rospy.Time.from_seconds(ts1) p.header.seq = lineno p.pose.position.x = rotation_matrix[0, 3] p.pose.position.y = rotation_matrix[1, 3] p.pose.position.z = rotation_matrix[2, 3] p.pose.orientation.x = quaternion[0] p.pose.orientation.y = quaternion[1] p.pose.orientation.z = quaternion[2] p.pose.orientation.w = quaternion[3] outputbag.write("tango_imu_pose", p, rospy.Time.from_seconds(ts1)) #creating the odometry entry #TODO get covariances from the covariances.txt file o = Odometry() o.header = p.header o.child_frame_id = o.header.frame_id o.pose.pose = p.pose outputbag.write("tango_imu_odom", o, rospy.Time.from_seconds(ts1)) #creating the tf messages tfmsg = tfMessage() tf_stamped = TransformStamped() tf_stamped.header.frame_id = "/tango/world" tf_stamped.header.seq = lineno tf_stamped.header.stamp = rospy.Time.from_seconds(ts1) tf_stamped.child_frame_id = "/tango/imu" tf_stamped.transform.translation.x = rotation_matrix[0, 3] tf_stamped.transform.translation.y = rotation_matrix[1, 3] tf_stamped.transform.translation.z = rotation_matrix[2, 3] tf_stamped.transform.rotation = p.pose.orientation tfmsg.transforms.append(tf_stamped) outputbag.write("tf", tfmsg, rospy.Time.from_seconds(ts1)) print("Bag creation complete, bagfile: {}".format(arguments.output))
def callback(data): odom = Odometry() odom.header = data.header odom.child_frame_id = child_frame_id odom.pose = data.pose pub.publish(odom)
def odom_remap(odom_msg): global namespace global previous_noiseless_pose global previous_noiseless_odom global previous_noiseless_time # Make our received odom reading more palatable current_odom = Pose2D() current_odom.x = odom_msg.pose.pose.position.x current_odom.y = odom_msg.pose.pose.position.y current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation) current_time = odom_msg.header.stamp if previous_noiseless_time is None: previous_noiseless_time = copy.deepcopy(current_time) # If we have no previous_pose, set to (0, 0, 0) if previous_noiseless_pose is None: previous_noiseless_pose = Pose2D() previous_noiseless_pose.x = 0 previous_noiseless_pose.y = 0 previous_noiseless_pose.theta = 0 # Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0) if previous_noiseless_odom is None: previous_noiseless_odom = Pose2D() previous_noiseless_odom.x = 0 previous_noiseless_odom.y = 0 previous_noiseless_odom.theta = 0 noiseless_odom = Odometry() noiseless_odom.header = odom_msg.header noiseless_odom.child_frame_id = namespace + 'base_footprint_filter' # Split movement into three distinct actions, rotate -> translate -> rotate delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noiseless_odom.y, current_odom.x - previous_noiseless_odom.x) - previous_noiseless_odom.theta) delta_translation = math.sqrt((previous_noiseless_odom.x - current_odom.x) ** 2 + (previous_noiseless_odom.y - current_odom.y) ** 2) delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noiseless_odom.theta - delta_rotation_1) noiseless_odom.pose.pose.position.x = previous_noiseless_pose.x + delta_translation * math.cos(previous_noiseless_pose.theta + delta_rotation_1) noiseless_odom.pose.pose.position.y = previous_noiseless_pose.y + delta_translation * math.sin(previous_noiseless_pose.theta + delta_rotation_1) noiseless_odom_yaw = previous_noiseless_pose.theta + delta_rotation_1 + delta_rotation_2 noiseless_odom.pose.pose.orientation = convert_yaw_to_quaternion(noiseless_odom_yaw) delta_time = current_time - previous_noiseless_time delta_time_seconds = delta_time.to_sec() assert delta_time_seconds >= 0 # We will assume that our robot moves perfectly with only linear x velocity and angular z velocity # This is an okay assumption because we ignore other data anyways because all readings are noise and not valid noiseless_odom.twist.twist.linear.y = 0 noiseless_odom.twist.twist.linear.z = 0 noiseless_odom.twist.twist.angular.x = 0 noiseless_odom.twist.twist.angular.y = 0 if delta_time_seconds > 0: noiseless_odom.twist.twist.linear.x = delta_translation / delta_time_seconds noiseless_odom.twist.twist.angular.z = (noiseless_odom_yaw - previous_noiseless_pose.theta) / delta_time_seconds else: noiseless_odom.twist.twist.linear.x = 0 noiseless_odom.twist.twist.angular.z = 0 # From documentation on nav_msgs/Odometry: # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) noiseless_odom.pose.covariance = odom_msg.pose.covariance # Twist covariance copied from position covariance TODO this definitely wrong noiseless_odom.twist.covariance = odom_msg.pose.covariance # Publish odom message try: odom_publisher.publish(noiseless_odom) except rospy.ROSException as e: rospy.logwarn(e.message) # Increment previous pose for use with next message previous_noiseless_pose.x = noiseless_odom.pose.pose.position.x previous_noiseless_pose.y = noiseless_odom.pose.pose.position.y previous_noiseless_pose.theta = convert_quaternion_to_yaw(noiseless_odom.pose.pose.orientation) # Increment previous odom for use with next message previous_noiseless_odom.x = current_odom.x previous_noiseless_odom.y = current_odom.y previous_noiseless_odom.theta = current_odom.theta # Increment time previous_noiseless_time = current_time
def noisy_odom_remap(odom_msg): global namespace global previous_noisy_pose global previous_noisy_odom global previous_noisy_time # Odometry model taken from Probabilistic Robotics by Thrun et al. # Algorithm used is sample_motion_model_odometry from Table 5.6 # Robot specific noise parameters # Default value = 0.02 alpha_1 = 0.02 alpha_2 = 0.02 alpha_3 = 0.02 alpha_4 = 0.02 # Make our received odom reading more palatable current_odom = Pose2D() current_odom.x = odom_msg.pose.pose.position.x current_odom.y = odom_msg.pose.pose.position.y current_odom.theta = convert_quaternion_to_yaw(odom_msg.pose.pose.orientation) current_time = odom_msg.header.stamp if previous_noisy_time is None: previous_noisy_time = copy.deepcopy(current_time) # If we have no previous_pose, set to (0, 0, 0) if previous_noisy_pose is None: previous_noisy_pose = Pose2D() previous_noisy_pose.x = 0 previous_noisy_pose.y = 0 previous_noisy_pose.theta = 0 # Create the previous_odom Pose2D if it doesn't exist, also (0, 0, 0) if previous_noisy_odom is None: previous_noisy_odom = Pose2D() previous_noisy_odom.x = 0 previous_noisy_odom.y = 0 previous_noisy_odom.theta = 0 noisy_odom_x_list = [] noisy_odom_y_list = [] noisy_odom_yaw_list = [] noisy_odom_x_vel_list = [] noisy_odom_y_vel_list = [] noisy_odom_yaw_vel_list = [] noisy_odom = Odometry() noisy_odom.header = odom_msg.header noisy_odom.child_frame_id = namespace + 'base_footprint_filter' for i in range(1, 1000): # Split movement into three distinct actions, rotate -> translate -> rotate delta_rotation_1 = helpers.correct_angle(math.atan2(current_odom.y - previous_noisy_odom.y, current_odom.x - previous_noisy_odom.x) - previous_noisy_odom.theta) delta_translation = math.sqrt((previous_noisy_odom.x - current_odom.x) ** 2 + (previous_noisy_odom.y - current_odom.y) ** 2) delta_rotation_2 = helpers.correct_angle(current_odom.theta - previous_noisy_odom.theta - delta_rotation_1) # Create random pose from given movements by adding noise std_dev_1 = alpha_1 * abs(delta_rotation_1) + alpha_2 * delta_translation delta_rotation_1_hat = delta_rotation_1 - normal(scale=std_dev_1) std_dev_2 = alpha_3 * delta_translation + alpha_4 * (abs(delta_rotation_1) + abs(delta_rotation_2)) delta_translation_hat = delta_translation - normal(scale=std_dev_2) std_dev_3 = alpha_1 * abs(delta_rotation_2) + alpha_2 * delta_translation delta_rotation_2_hat = delta_rotation_2 - normal(scale=std_dev_3) noisy_odom.pose.pose.position.x = previous_noisy_pose.x + delta_translation_hat * math.cos(previous_noisy_pose.theta + delta_rotation_1_hat) noisy_odom.pose.pose.position.y = previous_noisy_pose.y + delta_translation_hat * math.sin(previous_noisy_pose.theta + delta_rotation_1_hat) noisy_odom_yaw = previous_noisy_pose.theta + delta_rotation_1_hat + delta_rotation_2_hat noisy_odom.pose.pose.orientation = convert_yaw_to_quaternion(noisy_odom_yaw) delta_time = current_time - previous_noisy_time delta_time_seconds = delta_time.to_sec() assert delta_time_seconds >= 0 # We will assume that our robot moves perfectly with only linear x velocity and angular z velocity # This is an okay assumption because we ignore other data anyways because all readings are noise and not valid noisy_odom.twist.twist.linear.y = 0 noisy_odom.twist.twist.linear.z = 0 noisy_odom.twist.twist.angular.x = 0 noisy_odom.twist.twist.angular.y = 0 if delta_time_seconds > 0: noisy_odom.twist.twist.linear.x = delta_translation_hat / delta_time_seconds noisy_odom.twist.twist.angular.z = (noisy_odom_yaw - previous_noisy_pose.theta) / delta_time_seconds else: noisy_odom.twist.twist.linear.x = 0 noisy_odom.twist.twist.angular.z = 0 noisy_odom_x_list.append(noisy_odom.pose.pose.position.x) noisy_odom_y_list.append(noisy_odom.pose.pose.position.y) noisy_odom_yaw_list.append(noisy_odom_yaw) noisy_odom_x_vel_list.append(noisy_odom.twist.twist.linear.x) noisy_odom_y_vel_list.append(noisy_odom.twist.twist.linear.y) noisy_odom_yaw_vel_list.append(noisy_odom.twist.twist.angular.z) # From documentation on nav_msgs/Odometry: # Row-major representation of the 6x6 covariance matrix # The orientation parameters use a fixed-axis representation. # In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) noisy_odom.pose.covariance = [numpy.var(noisy_odom_x_list), 0, 0, 0, 0, 0, # x variance taken from Gazebo odometry 0, numpy.var(noisy_odom_y_list), 0, 0, 0, 0, # y variance taken from Gazebo odometry 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, numpy.var(noisy_odom_yaw_list)] # yaw variance taken from Gazebo odometry # Twist covariance copied from position covariance TODO this definitely wrong noisy_odom.twist.covariance = [numpy.var(noisy_odom_x_vel_list), 0, 0, 0, 0, 0, # x_vel variance 0, numpy.var(noisy_odom_y_vel_list), 0, 0, 0, 0, # y_vel variance 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, numpy.var(noisy_odom_yaw_vel_list)] # yaw_vel variance # Publish noisy odom message try: rospy.logdebug('Publishing noisy odom message') noisy_odom_publisher.publish(noisy_odom) except rospy.ROSException as e: rospy.logwarn(e.message) rospy.logdebug('Calculated new noisy odom pose. Previous pose was ' + str(previous_noisy_pose) + ',\n previous odom was ' + str(previous_noisy_odom) + ',\n current odom was ' + str(current_odom) + ',\n and new noisy pose is ' + str(noisy_odom)) rospy.logdebug('d_rot_1 noise was ' + str(delta_rotation_1_hat - delta_rotation_1) + '\n d_trans noise was ' + str(delta_translation_hat - delta_translation) + '\n d_rot_2 noise was ' + str(delta_rotation_2_hat - delta_rotation_2)) # Increment previous pose for use with next message previous_noisy_pose.x = noisy_odom.pose.pose.position.x previous_noisy_pose.y = noisy_odom.pose.pose.position.y previous_noisy_pose.theta = convert_quaternion_to_yaw(noisy_odom.pose.pose.orientation) # Increment previous odom for use with next message previous_noisy_odom.x = current_odom.x previous_noisy_odom.y = current_odom.y previous_noisy_odom.theta = current_odom.theta # Increment time previous_noisy_time = current_time
while not rospy.is_shutdown(): if opts.useoriginref: file_string = 'scan'+format(counter_index,'05d')+'.pcd' else: file_string = 'scanorg'+format(counter_index,'05d')+'.pcd' file_string = os.path.join(opts.dir_prefix, file_string) pcl_cloud = pcl.load(file_string) pcloud = PointCloud2() header = Header() header.stamp = rospy.Time.now() if opts.useoriginref: header.frame_id = 'map' else: header.frame_id = 'velodyne' #'odom' #'pose' #'frame' #'velodyne' pcloud = pc2.create_cloud_xyz32(header, pcl_cloud.to_array()) #pose = graph[str(counter_index)][0] index_graph = graph_node_names.index(str(counter_index)) pose = graph.nodes[index_graph].pose print pose odom = Odometry() odom.header = Header() odom.header.frame_id = 'map' odom.header.stamp = rospy.Time.now() odom.child_frame_id = 'velodyne' odom.pose.pose = pose pub_frame.publish(pcloud) pub_odom.publish(odom) pub_tf.sendTransform((pose.position.x, pose.position.y, pose.position.z), (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), rospy.Time.now(),'velodyne','map') counter_index = counter_index + 1 rate.sleep()
def callback(input): output = Odometry() output.header = input.header output.pose.pose = input.pose pub.publish(output)