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 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 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 publish_odometry(self, odom_combined): """ use current pose, along with delta from last pose to publish the current Odometry on the /odom topic """ if not self.last_time: # set up initial times and pose rospy.loginfo("Setting up initial position") self.last_time, self.last_x, y, self.last_theta = self.current_pose(odom_combined) return # publish to the /odom topic odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "/base_link" odom.pose = odom_combined.pose current_time, x, y, theta = self.current_pose(odom_combined) dt = current_time - self.last_time dt = dt.to_sec() d_x = x - self.last_x d_theta = theta - self.last_theta odom.twist.twist = Twist(Vector3(d_x/dt, 0, 0), Vector3(0, 0, d_theta/dt)) self.odom_publisher.publish(odom) self.last_time, self.last_x, self.last_theta = current_time, x, theta
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 svo_callback(self, data): # converts to nav_msg odometry type msg and publish odom = Odometry() odom.pose = data.pose odom.twist = self.twist self.pub_vo.publish(odom)
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 setupOdometry(self, data): odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.pose = self.packagePose(data) odom.twist = self.packageTwist(data) return odom
def __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler(0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin(math.radians(self.__heading)) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians(self.__rotation_per_second) * self.__twist.angular.z
def odom_callback(data): odom = Odometry() odom.header.frame_id = odom_frame odom.child_frame_id = base_frame odom.header.stamp = rospy.Time.now() odom.pose = data.pose odom.pose.pose.position.x = odom.pose.pose.position.x - 2.5 odom.pose.pose.position.y = odom.pose.pose.position.y + 7.0 odom.twist = data.twist tf = TransformStamped(header = Header( frame_id = odom.header.frame_id, stamp = odom.header.stamp), child_frame_id = odom.child_frame_id, transform = Transform( translation = odom.pose.pose.position, rotation = odom.pose.pose.orientation)) # visualize footprint everytime odom changes footprint_visualizer() odom_pub.publish(odom) tf_pub.sendTransform(tf)
def publishPose(): 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, 1.0 ] x = 0 y = 0 z = 0 qx = 0 qy = 0 qz = 0 qw = 1 time_now = rospy.Time.now() id_ = '/map' poseWCS = PoseWithCovarianceStamped() poseWCS.header.stamp = time_now poseWCS.header.frame_id = id_ poseWCS.pose.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWCS.pose.covariance = covariance_ initialpose_poseWCS_publisher.publish(poseWCS) poseWC = PoseWithCovariance() poseWC.pose = Pose(Point(x, y, z), Quaternion(qx, qy, qz, qw)) poseWC.covariance = covariance_ twistWC = TwistWithCovariance() twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) twistWC.covariance = covariance_ odom = Odometry() odom.header.stamp = time_now odom.header.frame_id = id_ odom.pose = poseWC odom.twist = twistWC fakelocalisation_poseWCS_publisher.publish(odom)
def main(self): sub = rospy.Subscriber('robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, self.pose_callback) sub = rospy.Subscriber('odom', Odometry, self.twist_callback) # Rate at which the program runs in (hz) how many times per second hz_rate = 15.0 current_time = rospy.Time.now() r = rospy.Rate(hz_rate) while not rospy.is_shutdown(): current_time = rospy.Time.now() # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "fake_odom" if self.pose_data != 0: odom.pose = self.pose_data odom.twist = self.twist_data odom.child_frame_id = "base_link" # publish the message self.localization_pub.publish(odom) r.sleep()
def GetOdomFromState(state, spot_wrapper, use_vision=True): """Maps odometry data from robot state proto to ROS Odometry message Args: data: Robot State proto spot_wrapper: A SpotWrapper object Returns: Odometry message """ odom_msg = Odometry() local_time = spot_wrapper.robotToLocalTime( state.kinematic_state.acquisition_timestamp) odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos) if use_vision == True: odom_msg.header.frame_id = 'vision' tform_body = get_vision_tform_body( state.kinematic_state.transforms_snapshot) else: odom_msg.header.frame_id = 'odom' tform_body = get_odom_tform_body( state.kinematic_state.transforms_snapshot) odom_msg.child_frame_id = 'body' pose_odom_msg = PoseWithCovariance() pose_odom_msg.pose.position.x = tform_body.position.x pose_odom_msg.pose.position.y = tform_body.position.y pose_odom_msg.pose.position.z = tform_body.position.z pose_odom_msg.pose.orientation.x = tform_body.rotation.x pose_odom_msg.pose.orientation.y = tform_body.rotation.y pose_odom_msg.pose.orientation.z = tform_body.rotation.z pose_odom_msg.pose.orientation.w = tform_body.rotation.w odom_msg.pose = pose_odom_msg twist_odom_msg = GetOdomTwistFromState(state, spot_wrapper).twist odom_msg.twist = twist_odom_msg return odom_msg
def __update_odometry(self, linear_offset, angular_offset, tf_delay): self.__heading = (self.__heading + angular_offset) % 360 q = tf.transformations.quaternion_from_euler( 0, 0, math.radians(self.__heading)) self.__pose.position.x += math.cos( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.y += math.sin( math.radians(self.__heading )) * self.__distance_per_cycle * self.__twist.linear.x self.__pose.position.z = 0.33 self.__pose.orientation = Quaternion(q[0], q[1], q[2], q[3]) now = rospy.Time.now() + rospy.Duration(tf_delay) self.__tfb.sendTransform( (self.__pose.position.x, self.__pose.position.y, self.__pose.position.z), q, now, 'base_link', 'odom') o = Odometry() o.header.stamp = now o.header.frame_id = 'odom' o.child_frame_id = 'base_link' o.pose = PoseWithCovariance(self.__pose, None) o.twist = TwistWithCovariance() o.twist.twist.linear.x = self.__distance_per_second * self.__twist.linear.x o.twist.twist.angular.z = math.radians( self.__rotation_per_second) * self.__twist.angular.z
def state_2_odom(state_vec, covariance, frame="odom"): out = Odometry() out.header.stamp = rospy.Time.now() out.header.frame_id = frame out.pose = state_to_pose_with_cov(state_vec, covariance) out.twist = state_to_twist_with_cov(state_vec, covariance) return out
def callback(data): print data odom = Odometry() odom.pose = data.pose odom.header.stamp = rospy.Time.now() odom.header.frame_id = "/odom" global pub pub.publish(odom)
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(self): trans = self.setupTransform(None) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.pose = PoseWithCovariance() odom.twist = TwistWithCovariance() self.tf_pub.sendTransform(trans) self.pub.publish(odom)
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 pose1_callback(self, data): pose = data.pose.pose self.bf1_pose = pose self.bf1_vel = data.twist.twist # publish a /odom_pose topic odom = Odometry() odom.pose = data.pose odom.twist = self.vel_cmd1.twist self.vehicle_odom_pub.publish(odom)
def callback( msg ): #define call back function, when there is data published on /odometry/odom the function will be called and processed data will be published on /mavros/odometry/out odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom_ned" odom.child_frame_id = "base_link_frd" odom.pose = msg.pose odom.twist = msg.twist publisher.publish(odom)
def odom_received(odom): global target_child_frame global new_odom_frame global publish_odometry_transforms global drift_vector global tfBroadcaster global tfBuffer global odom_publisher global first_stamp ros_transform = tfBuffer.lookup_transform(target_child_frame, odom.child_frame_id, odom.header.stamp) transform = ros_transform_to_matrix(ros_transform) pose = ros_pose_to_matrix(odom.pose.pose) twist = ros_twist_to_matrix(odom.twist.twist) new_pose = transform @ pose @ np.linalg.inv(transform) new_position = new_pose[:3, 3] new_orientation = mat2quat(new_pose[:3, :3]) new_twist = transform @ twist @ np.linalg.inv(transform) new_linear = new_twist[:3, 3] new_angular = skew_to_vector(new_twist[:3, :3]) if first_stamp == rospy.Time(0): first_stamp = odom.header.stamp new_position += drift_vector * (odom.header.stamp - first_stamp).to_sec() new_linear += drift_vector if odom_publisher is not None: new_odom = Odometry(header=deepcopy(odom.header)) new_odom.header.frame_id = new_odom_frame new_odom.child_frame_id = target_child_frame new_odom.pose = PoseWithCovariance(pose=Pose( Point(new_position[0], new_position[1], new_position[2]), Quaternion(new_orientation[1], new_orientation[2], new_orientation[3], new_orientation[0]))) new_odom.twist = TwistWithCovariance(twist=Twist( Vector3(new_linear[0], new_linear[1], new_linear[2]), Vector3(new_angular[0], new_angular[1], new_angular[2]))) odom_publisher.publish(new_odom) if publish_odometry_transforms: transform = TransformStamped(header=deepcopy(odom.header)) transform.header.frame_id = new_odom_frame transform.child_frame_id = target_child_frame transform.transform = Transform( Vector3(new_position[0], new_position[1], new_position[2]), Quaternion(new_orientation[1], new_orientation[2], new_orientation[3], new_orientation[0])) tfBroadcaster.sendTransform(transform)
def pose1_callback(self, data): pose = data.pose.pose self.bf1_pose = pose self.bf1_vel = data.twist.twist T_bf1 = tf.transformations.quaternion_matrix([ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ]) T_bf1[:3, 3] = np.array( [pose.position.x, pose.position.y, pose.position.z]) # calculate lidar pose T_lidar = np.dot( tf.transformations.rotation_matrix(np.pi / 2, direction=(0, 0, 1)), tf.transformations.rotation_matrix(np.pi / 2, direction=(1, 0, 0))) #T_lidar = tf.transformations.euler_matrix(-np.pi/2, -np.pi/2, 0) # T_lidar[0,3] += 0.2 T_lidar[1, 3] += 0.08 T_lidar[2, 3] += -0.15 T_lidar = np.dot(T_bf1, T_lidar) # # make lidar spin T_lidar = np.dot( T_lidar, tf.transformations.rotation_matrix(10. * self.lidar_counter, direction=(0, 1, 0))) self.lidar_counter += 1 if self.lidar_counter > 2000: self.lidar_counter = 0 quater_lidar = tf.transformations.quaternion_from_matrix(T_lidar) # make lidar follow lidar_odom = Odometry() lidar_odom.pose.pose.position.x = T_lidar[0, 3] lidar_odom.pose.pose.position.y = T_lidar[1, 3] lidar_odom.pose.pose.position.z = T_lidar[2, 3] lidar_odom.pose.pose.orientation.x = quater_lidar[0] lidar_odom.pose.pose.orientation.y = quater_lidar[1] lidar_odom.pose.pose.orientation.z = quater_lidar[2] lidar_odom.pose.pose.orientation.w = quater_lidar[3] self.lidar_pub.publish(lidar_odom) # publish a /odom_pose topic odom = Odometry() odom.pose = data.pose odom.twist = self.vel_cmd1.twist self.vehicle_odom_pub.publish(odom)
def get_origin_odom_msg(): """ Get ros odometry message at origin. Returns: Odometry: ROR odometry message with position = (0, 0, 0) and quaternion = (0, 0, 0, 1) """ msg = Odometry() pose = PoseWithCovariance() pose.pose.orientation.w = 1 msg.pose = pose return msg
def publish_sensors(self, msg): """ publishes noisy position values for each robot """ pwc = PoseWithCovariance() twc = TwistWithCovariance() auv = "akon" # X Meas rospy.loginfo_once("Normal Error on X") sigma = rospy.get_param("kalman/measurements/x_sigma") dot_sigma = rospy.get_param("kalman/measurements/x_dot_sigma") pwc.pose.position.x = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.x, sigma) pwc.covariance[0] = sigma**2 twc.twist.linear.x = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.x, dot_sigma) twc.covariance[0] = dot_sigma**2 # Y Meas rospy.loginfo_once("Normal Error on Y") sigma = rospy.get_param("kalman/measurements/y_sigma") dot_sigma = rospy.get_param("kalman/measurements/y_dot_sigma") pwc.pose.position.y = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.y, sigma) pwc.covariance[7] = sigma**2 twc.twist.linear.y = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.y, dot_sigma) twc.covariance[7] = dot_sigma**2 # Z Meas rospy.loginfo_once("Normal Error on Z") sigma = rospy.get_param("kalman/measurements/z_sigma") dot_sigma = rospy.get_param("kalman/measurements/z_dot_sigma") pwc.pose.position.z = np.random.normal( self.auvs[auv][ODOM_INDEX].pose.pose.position.z, sigma) pwc.covariance[14] = sigma**2 twc.twist.linear.z = np.random.normal( self.auvs[auv][ODOM_INDEX].twist.twist.linear.z, dot_sigma) twc.covariance[14] = dot_sigma**2 pwc.pose.orientation = self.auvs[auv][ODOM_INDEX].pose.pose.orientation odom_msg = Odometry() odom_msg.header.seq = self.seq odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "base_link" odom_msg.child_frame_id = odom_msg.header.frame_id odom_msg.pose = pwc odom_msg.twist = twc self.odom_sensor_pub.publish(odom_msg) self.seq += 1
def process_mocap_data(self): r = rospy.Rate(rate) while not rospy.is_shutdown(): if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"): t = self.tf.getLatestCommonTime(self.frame, "/world") z, q = self.tf.lookupTransform("/world", self.frame, t) #yawtest = tf.transformations.euler_from_quaternion(q)[2] self.kr3.step(z) #yaw = self.getYaw(q) #print yaw, yawtest #print roll_test pitch_test yawtest odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "/world" odom.child_frame_id = "/" + self.name + "/base_link" #quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw) posecov = PoseWithCovariance() pose = Pose() pose.position.x = self.kr3.pos[0] pose.position.y = self.kr3.pos[1] pose.position.z = self.kr3.pos[2] pose.orientation.x = q[0] pose.orientation.y = q[1] pose.orientation.z = q[2] pose.orientation.w = q[3] posecov.pose = pose posecov.covariance = [0] * 36 odom.pose = posecov twistcov = TwistWithCovariance() twist = Twist() twist.linear.x = self.kr3.vel[0] twist.linear.y = self.kr3.vel[1] twist.linear.z = self.kr3.vel[2] twist.angular.x = 0.0 twist.angular.y = 0.0 twist.angular.z = 0.0 twistcov.twist = twist twistcov.covariance = [0] * 36 odom.twist = twistcov self.odom_pub.publish(odom) poseStamped = PoseStamped() poseStamped.pose = pose poseStamped.header = odom.header poseStamped.header.frame_id = "/" + self.name + "/base_link" self.pose_pub.publish(poseStamped) r.sleep()
def convert(data): global prevTime; global prevPose; global isInitIter# = True; currentTime = rospy.Time.now(); if (isInitIter): prevTime = currentTime; dt = 99999999999999999; else: dt = currentTime.to_sec() - prevTime.to_sec(); #Get delta time. if (isInitIter): prevPose = data.pose; #Note first pose is a PoseWithCovariance. isInitIter = False; currentPose = PoseWithCovariance(); currentPose = data.pose; #Note first pose is a PoseWithCovariance. currentTwist = TwistWithCovariance(); #Calculate velocities. currentTwist.twist.linear.x = (currentPose.pose.position.x - prevPose.pose.position.x)/dt; currentTwist.twist.linear.y = (currentPose.pose.position.y - prevPose.pose.position.y)/dt; currentTwist.twist.linear.z = (currentPose.pose.position.z - prevPose.pose.position.z)/dt; currentTwist.twist.angular.x = (currentPose.pose.orientation.x - prevPose.pose.orientation.x)/dt; currentTwist.twist.angular.y = (currentPose.pose.orientation.y - prevPose.pose.orientation.y)/dt; currentTwist.twist.angular.z = (currentPose.pose.orientation.z - prevPose.pose.orientation.z)/dt; currentTwist.covariance = data.pose.covariance; # Make the message to publish: odomMessage = Odometry(); odomMessage.header = data.header; #odomMessage.header.frame_id = "something custom?" odomMessage.child_frame_id = "base_link"; odomMessage.pose = currentPose; odomMessage.twist = currentTwist; # publish the message p.publish(odomMessage); prevPose = currentPose;
def odom_callback(data): odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = 'odom' # 'map' odom.child_frame_id = 'base_link' # 'odom' odom.pose = data.pose odom.pose.pose.position.y = odom.pose.pose.position.y + 5.0 odom.twist = data.twist tf = TransformStamped(header=Header(frame_id=odom.header.frame_id, stamp=odom.header.stamp), child_frame_id=odom.child_frame_id, transform=Transform( translation=odom.pose.pose.position, rotation=odom.pose.pose.orientation)) odom_pub.publish(odom) tf_pub.sendTransform(tf)
def publish_wheel_odom(self): odom_msg = Odometry() odom_msg.header = Header() odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "odom" odom_msg.child_frame_id = "scout_1_tf/base_footprint" odom_msg.pose = PoseWithCovariance() odom_msg.pose.pose = Pose() odom_msg.pose.pose.position.x = self.wheel_odom[0] odom_msg.pose.pose.position.y = self.wheel_odom[1] #odom_msg.pose.pose.position.z = self.wheel_odom_z odom_msg.twist = TwistWithCovariance() odom_msg.twist.twist = Twist() self.wheel_odom_pub.publish(odom_msg)
def talker(): pub = rospy.Publisher("/mapping_planning/waypoints", Path, queue_size=10) pub2 = rospy.Publisher("/localization/pose1", Odometry, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz print("Publishing empty waypoints") while not rospy.is_shutdown(): #Create waypoint msg posestamped = PoseStamped() posestamped.header.stamp = rospy.Time.now() waypoints = Path() waypoints.header.stamp = rospy.Time.now() waypoints.poses.append(posestamped) waypoints.poses.append(posestamped) #Create odometry msg odometry_msg = Odometry() odometry_msg.header.stamp = rospy.Time.now() odometry_msg.header.frame_id = "ego_vehicle" odometry_msg.child_frame_id = "map" pose1 = PoseWithCovariance() pose2 = Pose() point = Point() point.x = 5 point.y = -45 point.z = 0.01 quat = Quaternion() quat.x = 0 quat.y = 0 quat.z = 0.7071 quat.w = 0.7071 pose2.position = point pose2.orientation = quat pose1.pose = pose2 odometry_msg.pose = pose1 #Publish msgs pub.publish(waypoints) pub2.publish(odometry_msg) rate.sleep()
def cbfake(self, msg): self.threadC.acquire() self.world.pose.pose.position.x = msg.pose.pose.position.x self.world.pose.pose.position.y = msg.pose.pose.position.y self.world.pose.pose.position.z = msg.pose.pose.position.z self.world.pose.pose.orientation.x = msg.pose.pose.orientation.x self.world.pose.pose.orientation.y = msg.pose.pose.orientation.y self.world.pose.pose.orientation.z = msg.pose.pose.orientation.z self.world.pose.pose.orientation.w = msg.pose.pose.orientation.w tmp1 = Odometry() tmp1.child_frame_id = "base_link" tmp1.pose = msg.pose tmp1.twist = msg.twist tmp1.header.frame_id = "odom" self.br.sendTransform((self.world.pose.pose.position.x,self.world.pose.pose.position.y,self.world.pose.pose.position.z)\ ,(self.world.pose.pose.orientation.x,self.world.pose.pose.orientation.y,self.world.pose.pose.orientation.z,self.world.pose.pose.orientation.w),rospy.Time.now(),"base_link","odom") self.pub.publish(tmp1) self.threadC.release()
def callback(pub, data): odom = Odometry() pose = PoseWithCovariance() twist = TwistWithCovariance() #no shift in relation to boat position pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = 0 #no rotation in relation to boat position pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 twist.twist = data odom.pose = pose odom.twist = twist odom.header.frame_id = "base_link" odom.child_frame_id = "base_link" pub.publish(odom)
def callback(data): odom = Odometry() odom.header = data.header odom.child_frame_id = child_frame_id odom.pose = data.pose pub.publish(odom)