def _msg(self, quaternion, x_dist, y_dist, linear_speed, angular_speed, now, use_pose_ekf=False): # next, we will publish the odometry message over ROS msg = Odometry() msg.header.frame_id = "odom" msg.header.stamp = now msg.pose.pose = Pose(Point(x_dist, y_dist, 0.0), quaternion) msg.child_frame_id = "base_link" msg.twist.twist = Twist(Vector3(linear_speed, 0, 0), Vector3(0, 0, angular_speed)) if use_pose_ekf: # robot_pose_ekf needs these covariances and we may need to adjust them. # From: ~/turtlebot/src/turtlebot_create/create_node/src/create_node/covariances.py # However, this is not needed because we are not using robot_pose_ekf # odometry.pose.covariance = [1e-3, 0, 0, 0, 0, 0, # 0, 1e-3, 0, 0, 0, 0, # 0, 0, 1e6, 0, 0, 0, # 0, 0, 0, 1e6, 0, 0, # 0, 0, 0, 0, 1e6, 0, # 0, 0, 0, 0, 0, 1e3] # # odometry.twist.covariance = [1e-3, 0, 0, 0, 0, 0, # 0, 1e-3, 0, 0, 0, 0, # 0, 0, 1e6, 0, 0, 0, # 0, 0, 0, 1e6, 0, 0, # 0, 0, 0, 0, 1e6, 0, # 0, 0, 0, 0, 0, 1e3] pass return 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 pubOdometry(self, event): odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = str(self.frame_id) odom.child_frame_id = "real_girona500" odom.pose.pose.position.x = self.p[0] odom.pose.pose.position.y = self.p[1] odom.pose.pose.position.z = self.p[2] orientation = tf.transformations.quaternion_from_euler(self.p[3], self.p[4], self.p[5], 'sxyz') odom.pose.pose.orientation.x = orientation[0] odom.pose.pose.orientation.y = orientation[1] odom.pose.pose.orientation.z = orientation[2] odom.pose.pose.orientation.w = orientation[3] #Haig de comentar les velocitats sino l'UWSim no va odom.twist.twist.linear.x = 0.0 #v_[0] odom.twist.twist.linear.y = 0.0 #v_[1] odom.twist.twist.linear.z = 0.0 #v_[2] odom.twist.twist.angular.x = 0.0 #v_[3] odom.twist.twist.angular.y = 0.0 #v_[4] odom.twist.twist.angular.z = 0.0 #v_[5] self.pub_odom.publish(odom) # Broadcast transform br = tf.TransformBroadcaster() br.sendTransform((self.p[0], self.p[1], self.p[2]), orientation, odom.header.stamp, odom.header.frame_id, "world")
def publish_odom(event): global motor_controller, od pos = motor_controller.position covariance = Config.get_covariance_matrix() odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" if rospy.get_time() - pos.position_time > 50.0/1000: odom.header.stamp = rospy.Time.now() else: odom.header.stamp = rospy.Time.from_sec(pos.position_time) odom.pose.pose.position.x = pos.x odom.pose.pose.position.y = pos.y odom.pose.pose.orientation.z = math.sin(pos.yaw / 2) odom.pose.pose.orientation.w = math.cos(pos.yaw / 2) odom.pose.covariance = covariance odom.twist.twist.linear.x = pos.linear_vel odom.twist.twist.angular.z = pos.angular_vel odom.twist.covariance = covariance odom_publisher.publish(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 tf_and_odom(self, zumo_msg): """Broadcast tf transform and publish odometry. It seems a little redundant to do both, but both are needed if we want to use the ROS navigation stack.""" trans = (zumo_msg.x, zumo_msg.y, 0) rot = tf.transformations.quaternion_from_euler(0, 0, zumo_msg.theta) self.broadcaster.sendTransform(trans, rot, \ rospy.Time.now(), \ "base_link", \ "odom") odom = Odometry() odom.header.frame_id = 'odom' odom.header.stamp = rospy.Time.now() odom.pose.pose.position.x = trans[0] odom.pose.pose.position.y = trans[1] odom.pose.pose.position.z = trans[2] odom.pose.pose.orientation.x = rot[0] odom.pose.pose.orientation.y = rot[1] odom.pose.pose.orientation.z = rot[2] odom.pose.pose.orientation.w = rot[3] odom.child_frame_id = 'base_link' odom.twist.twist = self.stored_twist self.odomPublisher.publish(odom)
def update_vel(): "Main loop""" global g_last_loop_time, g_vel_x, g_vel_y, g_vel_dt, x_pos, y_pos #while not rospy.is_shutdown(): current_time = rospy.Time.now() linear_velocity_x = g_vel_x linear_velocity_y = g_vel_y # Calculate current position of the robot x_pos += linear_velocity_x * g_vel_dt y_pos += linear_velocity_y * g_vel_dt # All odometry is 6DOF, so need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, 0) # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" # set the position odom.pose.pose = Pose(Point(x_pos, y_pos, 0.), Quaternion(*odom_quat)) # set the velocity odom.child_frame_id = "base_footprint" odom.twist.twist = Twist( Vector3(linear_velocity_x, linear_velocity_y, 0), Vector3(0, 0, 0)) # publish the message odom_pub.publish(odom) g_last_loop_time = current_time
def handle_harlie_pose(msg, push_casters): pose = changePoseCoordFrame(msg) current_time = rospy.Time.now() theta = pose.theta + pi if push_casters else pose.theta quaternion = tf.transformations.quaternion_about_axis(theta, (0,0,1)) tf_br.sendTransform(translation = (pose.x, pose.y, 0), rotation = tuple(quaternion), time = current_time, child = 'base_link', parent = 'odom') odom_msg = Odometry() odom_msg.header.stamp = current_time odom_msg.header.frame_id = 'odom' odom_msg.pose.pose.position.x = pose.x odom_msg.pose.pose.position.y = pose.y odom_msg.pose.pose.position.z = 0.0 odom_msg.pose.pose.orientation = Quaternion(*quaternion) odom_msg.pose.covariance[0] = pose.x_var odom_msg.pose.covariance[7] = pose.y_var odom_msg.pose.covariance[35] = pose.theta_var odom_msg.child_frame_id = 'base_link' odom_msg.twist.twist.linear.x = pose.vel odom_msg.twist.twist.angular.z = pose.omega odom_msg.twist.covariance[0] = pose.vel_var odom_msg.twist.covariance[35] = pose.omega_var odom_pub.publish(odom_msg)
def Talk(self): msgs = Odometry() msgs.header.stamp = rospy.Time.now() msgs.header.frame_id = '/nav' msgs.child_frame_id = self.name msgs.pose.pose.position.x = self.position.x msgs.pose.pose.position.y = self.position.y msgs.pose.pose.position.z = self.position.z msgs.pose.pose.orientation.x = self.orientation.x msgs.pose.pose.orientation.y = self.orientation.y msgs.pose.pose.orientation.z = self.orientation.z msgs.pose.pose.orientation.w = self.orientation.w msgs.twist.twist.linear.x = self.velocity.x msgs.twist.twist.linear.y = self.velocity.y msgs.twist.twist.linear.z = self.velocity.z #rospy.loginfo(msgs) self.publisher['state'].publish(msgs) self.tf_broadcaster['local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , (msgs.pose.pose.orientation.x, msgs.pose.pose.orientation.y, msgs.pose.pose.orientation.z, msgs.pose.pose.orientation.w), msgs.header.stamp, msgs.child_frame_id, msgs.header.frame_id)
def getOdometry(odometry_line): # Timestamp [seconds.microseconds] # Rolling Counter [signed 16bit integer] # TicksRight [ticks] # TicksLeft [ticks] # X [m]* # Y [m]* # Theta [rad]* # => 1235603339.32339, 4103, 2464, 2559, 1.063, 0.008, -0.028 str_x, str_y, str_th = odometry_line.split(', ')[4:7] current_time = rospy.Time.now() th = float(str_th) x = float(str_x) y = float(str_y) z = 0.0 roll = 0 pitch = 0 yaw = th qx, qy, qz, qw = tf.transformations.quaternion_from_euler(roll, pitch, yaw) # BUILDING ODOMETRY msg = Odometry() msg.header.stamp = current_time msg.header.frame_id = 'odom' msg.child_frame_id = 'base_link' msg.pose.pose.position = Point(x, y, z) msg.pose.pose.orientation = Quaternion(qx, qy, qz, qw) return msg
def handle_imu(self, imu_data): if not self.imu_recv: rospy.loginfo('IMU data received') self.imu_recv = True if self.pub.get_num_connections() != 0: msg = Odometry() #msg.header.frame_id = imu_data.header.frame_id msg.header.frame_id = 'imu_frame' msg.header.stamp = imu_data.header.stamp msg.child_frame_id = 'imu_base_link' msg.pose.pose.position.x = 0.0 # here, we can maybe try to integrate accelerations... msg.pose.pose.position.y = 0.0 msg.pose.pose.position.z = 0.0 msg.pose.pose.orientation = imu_data.orientation # should we transform this orientation into base_footprint frame???? msg.pose.covariance = [99999, 0, 0, 0, 0, 0, # large covariance on x, y, z 0, 99999, 0, 0, 0, 0, 0, 0, 99999, 0, 0, 0, 0, 0, 0, 1e-06, 0, 0, # we trust in rpy 0, 0, 0, 0, 1e-06, 0, 0, 0, 0, 0, 0, 1e-06] # does robot_pose_ekf care about twist? self.pub.publish(msg)
def publish_Tfs(): global tf_rot,tf_trans,tf_tda,tf_rda,tf_tbc,tf_rbc,scale br = tf.TransformBroadcaster() rate = rospy.Rate(50.0) while not rospy.is_shutdown(): rate.sleep() camOdom = Odometry() camOdom.header.stamp = rospy.Time.now() camOdom.header.frame_id = "/odom_combined" camOdom.pose.pose.position.x =0 camOdom.pose.pose.position.y =0 camOdom.pose.pose.position.z =0. #Tbc[2] M=np.identity(4) q=tf.transformations.quaternion_from_matrix(M) camOdom.pose.pose.orientation.x = q[0] camOdom.pose.pose.orientation.y = q[1] camOdom.pose.pose.orientation.z = q[2] camOdom.pose.pose.orientation.w = q[3] camOdom.child_frame_id = "/base_footprint" camOdom.twist.twist.linear.x = 0 camOdom.twist.twist.linear.y = 0 camOdom.twist.twist.angular.z = 0 T=np.array([0.,0.,0.]) M=np.identity(4) q=tf.transformations.quaternion_from_matrix(M) br.sendTransform(T,q, rospy.Time.now(), "/base_footprint", "/odom_combined") camOdomPub.publish(camOdom)
def poll(self): (x, y, theta) = self.arduino.arbot_read_odometry() quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(theta / 2.0) quaternion.w = cos(theta / 2.0) # Create the odometry transform frame broadcaster. now = rospy.Time.now() self.odomBroadcaster.sendTransform( (x, y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), now, "base_link", "odom" ) odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.header.stamp = now odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = self.forwardSpeed odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.angularSpeed self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed) self.odomPub.publish(odom)
def onNavSts(self,data): q = tf.transformations.quaternion_from_euler(math.pi, 0, math.pi/2) self.broadcaster.sendTransform((0,0,0), q.conjugate(), rospy.Time.now(), "uwsim_frame", "local"); try: (trans,rot) = self.listener.lookupTransform("uwsim_frame", self.base_frame, rospy.Time(0)) odom = Odometry(); odom.twist.twist.linear.x = data.body_velocity.x; odom.twist.twist.linear.y = data.body_velocity.y; odom.twist.twist.linear.z = data.body_velocity.z; odom.twist.twist.angular.x = data.orientation_rate.roll; odom.twist.twist.angular.y = data.orientation_rate.pitch; odom.twist.twist.angular.z = data.orientation_rate.yaw; odom.child_frame_id = self.base_frame; odom.pose.pose.orientation.x = rot[0]; odom.pose.pose.orientation.y = rot[1]; odom.pose.pose.orientation.z = rot[2]; odom.pose.pose.orientation.w = rot[3]; odom.pose.pose.position.x = trans[0]; odom.pose.pose.position.y = trans[1]; odom.pose.pose.position.z = trans[2]; odom.header.stamp = rospy.Time.now(); odom.header.frame_id = "local"; self.pub.publish(odom); except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): None
def callback(msg): pub = rospy.Publisher("odom_covar", Odometry) corrected = Odometry() corrected.header.seq = msg.header.seq corrected.header.stamp = rospy.Time.now() corrected.header.frame_id = msg.header.frame_id corrected.child_frame_id = "base_footprint" corrected.pose.pose = msg.pose.pose corrected.pose.covariance = [.01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01] corrected.twist.twist = msg.twist.twist corrected.twist.covariance = [.01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01, 0, 0, 0, 0, 0, 0, .01] pub.publish(corrected)
def post_odometry(self, component_instance): """ Publish the data of the Pose as a Odometry message for fake localization """ parent_name = component_instance.robot_parent.blender_obj.name component_name = component_instance.blender_obj.name frame_id = self._properties[component_name]['frame_id'] child_frame_id = self._properties[component_name]['child_frame_id'] odometry = Odometry() odometry.header.stamp = rospy.Time.now() odometry.header.frame_id = frame_id odometry.child_frame_id = child_frame_id odometry.pose.pose.position.x = component_instance.local_data['x'] odometry.pose.pose.position.y = component_instance.local_data['y'] odometry.pose.pose.position.z = component_instance.local_data['z'] euler = mathutils.Euler((component_instance.local_data['roll'], component_instance.local_data['pitch'], component_instance.local_data['yaw'])) quaternion = euler.to_quaternion() odometry.pose.pose.orientation = quaternion for topic in self._topics: # publish the message on the correct topic if str(topic.name) == str("/" + parent_name + "/" + component_name): topic.publish(odometry)
def Talk(self, time): #print self.orientation.get_euler()['yaw'], self.position.yaw, self.sensors['gps'].position.yaw msgs = Odometry( ) msgs.header.stamp = rospy.Time.now() msgs.header.frame_id = "/nav" msgs.child_frame_id = "/drone_local" msgs.pose.pose.position.x = self.position.x msgs.pose.pose.position.y = self.position.y msgs.pose.pose.position.z = self.position.z msgs.pose.pose.orientation.x = self.orientation.x msgs.pose.pose.orientation.y = self.orientation.y msgs.pose.pose.orientation.z = self.orientation.z msgs.pose.pose.orientation.w = self.orientation.w msgs.twist.twist.linear.x = self.velocity.x msgs.twist.twist.linear.y = self.velocity.y msgs.twist.twist.linear.z = self.velocity.z #rospy.loginfo(msgs) self.publisher['state'].publish(msgs) self.tf_broadcaster['drone_local_tf'].sendTransform( (msgs.pose.pose.position.x, msgs.pose.pose.position.y, msgs.pose.pose.position.z) , self.orientation.get_quaternion( ), msgs.header.stamp, msgs.child_frame_id, msgs.header.frame_id)
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth): quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta) current_time = rospy.Time.now() br = tf.TransformBroadcaster() br.sendTransform((cur_x, cur_y, 0), tf.transformations.quaternion_from_euler(0, 0, cur_theta), current_time, "base_footprint", "odom") odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = 'odom' odom.pose.pose.position.x = cur_x odom.pose.pose.position.y = cur_y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = Quaternion(*quat) odom.pose.covariance[0] = 0.01 odom.pose.covariance[7] = 0.01 odom.pose.covariance[14] = 99999 odom.pose.covariance[21] = 99999 odom.pose.covariance[28] = 99999 odom.pose.covariance[35] = 0.01 odom.child_frame_id = 'base_footprint' odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth odom.twist.covariance = odom.pose.covariance self.odom_pub.publish(odom)
def publish_odom(self, robot): # Transform etc x = robot.x y = robot.y th = robot.th br = tf.TransformBroadcaster() quat = tf.transformations.quaternion_from_euler(0,0,th) odom_trans = TransformStamped() odom_trans.header.stamp = rospy.Time.now() odom_trans.header.frame_id = "/map" odom_trans.child_frame_id = "/base_link" odom_trans.transform.translation.x = x odom_trans.transform.translation.y = y odom_trans.transform.translation.z = 0.0 odom_trans.transform.rotation = quat; #tf.TransformBroadcaster().SendTransform(R,t,time,child,parent) br.sendTransform((x,y,0.0), quat, odom_trans.header.stamp, odom_trans.child_frame_id, odom_trans.header.frame_id) # Publish Odom msg odom_msg = Odometry() odom_msg.header.stamp = rospy.Time.now() odom_msg.header.frame_id = "/map" odom_msg.child_frame_id = "/base_link" odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y odom_msg.pose.pose.position.z = 0.0 odom_msg.pose.pose.orientation.x = quat[0] odom_msg.pose.pose.orientation.y = quat[1] odom_msg.pose.pose.orientation.z = quat[2] odom_msg.pose.pose.orientation.w = quat[3] # publish self.odomPub.publish(odom_msg)
def main(): rospy.init_node("pose2d_to_odometry") own_num = rospy.get_param('~own_num', 0) global odom, head head = Header() head.stamp = rospy.Time.now() head.frame_id = "robot_{}/odom_combined".format(own_num) odom = Odometry() odom.child_frame_id = "robot_{}/base_footprint".format(own_num) o = 10**-9 # zero odom.pose.covariance = [1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, o, 0, 0, 0, # z doesn't change 0, 0, 0, o, 0, 0, # constant roll 0, 0, 0, 0, o, 0, # constant pitch 0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov odom.twist.covariance = [100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, # large covariances - no data 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100] global odom_pub odom_topic = rospy.get_param('~odometry_publish_topic', 'vo') pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D') odom_pub = rospy.Publisher(odom_topic, Odometry) rospy.Subscriber(pose2D_topic, Pose2D, on_pose) rospy.spin()
def Publish_Odom_Tf(self): #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0 quaternion.y = 0 quaternion.z = math.sin(self.Heading / 2.0) quaternion.w = math.cos(self.Heading / 2.0) rosNow = rospy.Time.now() self._tf_broad_caster.sendTransform( (self.X_Pos, self.Y_Pos, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = self.X_Pos odometry.pose.pose.position.y = self.Y_Pos odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.Velocity odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.Omega self._odom_publisher.publish(odometry)
def talker(message): corrected = Odometry() corrected.header.seq = message.header.seq corrected.header.stamp = rospy.Time.now() corrected.header.frame_id = message.header.frame_id #base_footprint #message.header.frame_id corrected.child_frame_id = message.child_frame_id #"base_footprint" corrected.pose.pose = message.pose.pose corrected.pose.covariance = [0.05, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, .5] corrected.twist.twist = message.twist.twist corrected.twist.covariance = [0.05, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, .5] pub = rospy.Publisher('odom_w_cov', Odometry) pub.publish(corrected)
def listener(): rospy.init_node('listener', anonymous=True) rospy.Subscriber("/wiimote1/imu", Imu, wiimote1Callback) rospy.Subscriber("/wiimote2/imu", Imu, wiimote2Callback) odomPub = rospy.Publisher("/base_controller/odom", Odometry) odom = Odometry() while 1: br = tf.TransformBroadcaster() time.sleep(.01) robot.updateState() if robot.readyToPublish: quaternion = tf.transformations.quaternion_from_euler(0, 0, robot.theta) #tf msg #br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/odom", "/map") br.sendTransform((robot.x, robot.y, 0), tf.transformations.quaternion_from_euler(0, 0, robot.theta),rospy.Time.now(),"/base_link", "/odom") br.sendTransform((0, 0, .7), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/base_laser_link", "/base_link") br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0),rospy.Time.now(),"/camera1_link", "/base_laser_link") br.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 3.14),rospy.Time.now(),"/camera2_link", "/base_laser_link") robot.readyToPublish = False #odometry msg odom.header.stamp = robot.lastTime odom.child_frame_id = "base_link" odom.pose.pose.position.x = robot.x odom.pose.pose.position.y = robot.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation.x = quaternion[0] odom.pose.pose.orientation.y = quaternion[1] odom.pose.pose.orientation.z = quaternion[2] odom.pose.pose.orientation.w = quaternion[3] odom.twist.twist.linear.x = robot.xV odom.twist.twist.linear.y = robot.yV # the x angular velocity corresponds to left wheel and y to right wheel odom.twist.twist.angular.x = robot.leftV odom.twist.twist.angular.y = robot.rightV odom.twist.twist.angular.z = robot.thetaV # place holder covariance matrix P = [.0001, 0, 0, 0, 0, 0, 0, .0001, 0, 0, 0, 0, 0, 0, .0001, 0, 0, 0, 0, 0, 0, .0001, 0, 0, 0, 0, 0, 0, .0001, 0, 0, 0, 0, 0, 0, .0001] odom.pose.covariance = P odomPub.publish(odom) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def publish(self, data): q = Quaternion() q.x = 0 q.y = 0 q.z = data[6] q.w = data[7] odomMsg = Odometry() odomMsg.header.frame_id = self._odom odomMsg.header.stamp = rospy.get_rostime() odomMsg.child_frame_id = self._baseLink odomMsg.pose.pose.position.x = data[0] odomMsg.pose.pose.position.y = data[1] odomMsg.pose.pose.position.z = 0 odomMsg.pose.pose.orientation = q if self._firstTimePub: self._prevOdom = odomMsg self._firstTimePub = False return velocity = Twist() deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec() yaw, pitch, roll = euler_from_quaternion( [odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y, odomMsg.pose.pose.orientation.z]) prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion( [self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x, self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z]) if deltaTime > 0: velocity.linear.x = (data[8] / deltaTime) deltaYaw = yaw - prevYaw # rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw)) if deltaYaw > math.pi: deltaYaw -= 2 * math.pi elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi if deltaTime > 0: velocity.angular.z = -(deltaYaw / deltaTime) # rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z)) odomMsg.twist.twist = velocity self._prevOdom = odomMsg traMsg = TransformStamped() traMsg.header.frame_id = self._odom traMsg.header.stamp = rospy.get_rostime() traMsg.child_frame_id = self._baseLink traMsg.transform.translation.x = data[0] traMsg.transform.translation.y = data[1] traMsg.transform.translation.z = 0 traMsg.transform.rotation = q self._pub.publish(odomMsg) self._broadCase.sendTransformMessage(traMsg)
def inspvax_handler(self, inspvax): # Convert the latlong to x,y coordinates and publish an Odometry try: utm_pos = geodesy.utm.fromLatLong(inspvax.latitude, inspvax.longitude) except ValueError: # Probably coordinates out of range for UTM conversion. return if not self.init and self.zero_start: self.origin.x = utm_pos.easting self.origin.y = utm_pos.northing self.origin.z = inspvax.altitude self.pub_origin.publish(position=self.origin) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = self.odom_frame odom.child_frame_id = self.base_frame odom.pose.pose.position.x = utm_pos.easting - self.origin.x odom.pose.pose.position.y = utm_pos.northing - self.origin.y odom.pose.pose.position.z = inspvax.altitude - self.origin.z # Orientation # Save this on an instance variable, so that it can be published # with the IMU message as well. self.orientation = tf.transformations.quaternion_from_euler( radians(inspvax.pitch), radians(inspvax.roll), radians(90 - inspvax.azimuth) ) print inspvax.azimuth odom.pose.pose.orientation = Quaternion(*self.orientation) odom.pose.covariance[21] = self.orientation_covariance[0] = pow(2, inspvax.pitch_std) odom.pose.covariance[28] = self.orientation_covariance[4] = pow(2, inspvax.roll_std) odom.pose.covariance[35] = self.orientation_covariance[8] = pow(2, inspvax.azimuth_std) # Twist is relative to vehicle frame odom.twist.twist.linear.x = inspvax.east_velocity odom.twist.twist.linear.y = inspvax.north_velocity odom.twist.twist.linear.z = inspvax.up_velocity TWIST_COVAR[0] = pow(2, inspvax.east_velocity_std) TWIST_COVAR[7] = pow(2, inspvax.north_velocity_std) TWIST_COVAR[14] = pow(2, inspvax.up_velocity_std) # odom.twist.twist.angular = imu.angular_velocity odom.twist.covariance = TWIST_COVAR self.pub_odom.publish(odom) # Odometry transform (if required) if self.publish_tf: self.tf_broadcast.sendTransform( (odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z), self.orientation, odom.header.stamp, odom.child_frame_id, odom.header.frame_id, ) # Mark that we've received our first fix, and set origin if necessary. self.init = True
def run(self): rosRate = rospy.Rate(self.rate) rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz") vx = 0.0 vy = 0.0 vth = 0.0 while not rospy.is_shutdown() and not self.finished.isSet(): now = rospy.Time.now() if now > self.t_next: dt = now - self.then self.then = now dt = dt.to_sec() delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt delta_th = vth * dt self.x += delta_x self.y += delta_y self.th += delta_th quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), "base_link", "odom" ) odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth self.odomPub.publish(odom) self.t_next = now + self.t_delta rosRate.sleep()
def advance(self, dt): now = self.then + dt elapsed = now - self.then self.then = now if self.debug: print "advancing to t=%5.3f" % (self.then) ldist = dt * self.lwSpeed rdist = dt * self.rwSpeed d = (ldist + rdist)/2.0 th = (rdist - ldist)/self.distanceBetweenWheels dx = d / dt dth = th / dt if (d != 0): x = math.cos(th)*d y = math.sin(th)*d # Does this differ from the create driver? Should it? self.x = self.x + (math.cos(self.th)*x - math.sin(self.th)*y) self.y = self.y + (math.sin(self.th)*x + math.cos(self.th)*y) if (th != 0): self.th = self.th + th self.th = self.th % (2 * math.pi) self.th = self.th - (2 * math.pi) if self.th > math.pi else self.th quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = math.sin(self.th/2.0) quaternion.w = math.cos(self.th/2.0) # Most of the applications I'm working on don't need this. # self.odomBroadcaster.sendTransform( # (self.x, self.y, 0), # (quaternion.x, quaternion.y, quaternion.z, quaternion.w), # rospy.Time.now(), # "base_link", # "odom" # ) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = "base_link" odom.twist.twist.linear.x = dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = dth self.odomPub.publish(odom)
def publish_odometry(self): quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw) quaternion = Quaternion() quaternion.x = quat[0] quaternion.y = quat[1] quaternion.z = quat[2] quaternion.w = quat[3] if self.last_time is None: self.last_time = rospy.Time.now() vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_; vy = self.vy #msg->twist.twist.linear.y; vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_; current_time = rospy.Time.now() dt = (current_time - self.last_time).to_sec() delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt delta_th = vth * dt self.x += delta_x self.y += delta_y self.yaw += delta_th odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = vy odom.twist.twist.angular.z = vth self.odom_pub.publish(odom) transform_stamped = TransformStamped() transform_stamped.header = odom.header transform_stamped.transform.translation.x = self.x transform_stamped.transform.translation.y = self.y transform_stamped.transform.translation.z = 0.0 transform_stamped.transform.rotation = quaternion #self.tfbr.sendTransform(transform_stamped) self.tfbr.sendTransform((self.x, self.y, 0.0), (quat[0], quat[1], quat[2], quat[3]), rospy.Time.now(), "odom", "base_link") self.last_time = current_time
def update(self): ############################################################################# now = rospy.Time.now() if now > self.t_next: elapsed = now - self.then self.then = now elapsed = elapsed.to_sec() # this approximation works (in radians) for small angles th = self.th - self.th_pre self.dr = th / elapsed # publish the odom information quaternion = Quaternion() quaternion.x = self.qx quaternion.y = self.qy quaternion.z = self.qz quaternion.w = self.qw self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (0, 0, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame_id, self.odom_frame_id, ) self.laserBroadcaster.sendTransform( (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id ) odom = Odometry() odom.header.stamp = now odom.header.frame_id = self.odom_frame_id odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.child_frame_id = self.base_frame_id odom.twist.twist.linear.x = self.dx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = self.dr self.odomPub.publish(odom) laser = LaserScan() laser.header.stamp = now laser.header.frame_id = self.laser_frame_id laser.angle_min = self.laser.angle_min laser.angle_max = self.laser.angle_max laser.angle_increment = self.laser.angle_increment laser.time_increment = self.laser.time_increment laser.scan_time = self.laser.scan_time laser.range_min = self.laser.range_min laser.range_max = self.laser.range_max laser.ranges = self.laser.ranges laser.intensities = self.laser.intensities self.laserPub.publish(laser)
def _BroadcastOdometryInfo(self, lineParts): partsCount = len(lineParts) #rospy.logwarn(partsCount) if (partsCount < 6): pass try: x = int(lineParts[1]) / 1000.0 y = int(lineParts[2]) / 1000.0 theta = int(lineParts[3]) / 1000.0 vx = int(lineParts[4]) / 1000.0 omega = int(lineParts[5]) / 1000.0 #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(theta / 2.0) quaternion.w = cos(theta / 2.0) rosNow = rospy.Time.now() # First, we'll publish the transform from frame odom to frame base_link over tf # Note that sendTransform requires that 'to' is passed in before 'from' while # the TransformListener' lookupTransform function expects 'from' first followed by 'to'. self._OdometryTransformBroadcaster.sendTransform( (x, y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_link", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = x odometry.pose.pose.position.y = y odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = vx odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = omega self._OdometryPublisher.publish(odometry) #rospy.loginfo(odometry) except: rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
def read_data(self, event): data_str = self.ard.readline() data_list = data_str.split() try: data_list = [float(i) / 100 for i in data_list] except ValueError: return # incorrect data #print data_list if len(data_list) != 4: return # incorrect array size if not in_range(data_list, -5.0, 5.0): # 36RPM -> 3.7699 rad/s return # data not in range self.w_fl, self.w_fr, self.w_rl, self.w_rr = data_list # dead reckoning dt = rospy.Time.now().to_sec() - self.time.to_sec() # time difference self.time = rospy.Time.now() # update time self.v_x = (self.w_fl + self.w_fr + self.w_rl + self.w_rr) * R / 4 self.v_y = (-self.w_fl + self.w_fr + self.w_rl - self.w_rr) * R / 4 self.omega = (-self.w_fl + self.w_fr - self.w_rl + self.w_rr) * R / 4 / (LX + LY) print[self.v_x, self.v_y, self.omega] self.x = self.x + self.v_x * dt self.y = self.y + self.v_y * dt self.heading = self.heading + self.omega * dt if self.pub_tf: self.tf_br.sendTransform( (self.x, self.y, 0), (0, 0, sin(self.heading / 2), cos(self.heading / 2)), rospy.Time.now(), 'car_base', 'odom') # Publish odometry message odom = Odometry() odom.header.frame_id = 'odom' odom.child_frame_id = 'car_base' odom.pose.pose.orientation.z = sin(self.heading / 2) odom.pose.pose.orientation.w = cos(self.heading / 2) odom.twist.twist.linear.x = self.v_x odom.twist.twist.linear.y = self.v_y self.pub_odom.publish(odom)
def publish_odom(x, pub): """ Convert quad state into an odometry message and publish it. :param x: :param pub: """ # Roll pitch yaw trust odom = Odometry() odom.pose.pose.position.x = x[0] odom.pose.pose.position.y = x[1] odom.pose.pose.position.z = x[2] # Velocities odom.twist.twist.linear.x = x[3] odom.twist.twist.linear.y = x[4] odom.twist.twist.linear.z = x[5] # Orientation odom.pose.pose.orientation.x = x[6] odom.pose.pose.orientation.y = x[7] odom.pose.pose.orientation.z = x[8] odom.pose.pose.orientation.w = x[9] odom.pose.pose.orientation.x = x[7] odom.pose.pose.orientation.y = x[8] odom.pose.pose.orientation.z = x[9] odom.pose.pose.orientation.w = x[6] # Angular velocities odom.twist.twist.angular.x = x[10] odom.twist.twist.angular.y = x[11] odom.twist.twist.angular.z = x[12] odom.child_frame_id = 'modquad' odom.header.frame_id = 'world' pub.publish(odom)
def __init__(self): self.set_init_pose = 0 self.init_pose = None self.curr_pose = None self.init_twist = None self.curr_twist = None self.rover_name = rospy.get_param("rover_name", "scout_1") rospy.Subscriber("/gazebo/model_states", ModelStates, callback=self.model_states_callback) self.gt_pub = rospy.Publisher("/debugging/gt_odom", Odometry, queue_size=5) self.init_pose_pub = rospy.Publisher("/debugging/init_pose", Pose, queue_size=5) rospy.init_node('rover_localization_gt') rate = rospy.Rate(30) # 30hz while not rospy.is_shutdown(): if self.curr_pose is not None and self.curr_twist is not None: 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 = self.rover_name odom_msg.pose = PoseWithCovariance() odom_msg.pose.pose = self.curr_pose odom_msg.twist = TwistWithCovariance() odom_msg.twist.twist = self.curr_twist self.gt_pub.publish(odom_msg) if self.init_pose is not None and self.init_twist is not None: self.init_pose_pub.publish(self.init_pose) rate.sleep()
def callback_odom(self, msg): x = msg.pose.pose.position.x y = msg.pose.pose.position.y z = msg.pose.pose.position.z (r, p, y) = tf.transformations.euler_from_quaternion([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) # (r, p, y) = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z) meas = np.array([x, y, z, r, p, y]) self.buffer_meas[self.pointer, :] = meas self.pointer = self.pointer + 1 if self.pointer == self.n_meas: self.full_buffer = True self.pointer = 0 if self.full_buffer: A = np.cov(self.buffer_meas.T) for a in A: print(a) print("------------") cov_mat = np.cov(self.buffer_meas.T).flatten() output = Odometry() output.header.frame_id = 'map' output.child_frame_id = 'zed_center' output.header.stamp = rospy.get_rostime() output.pose.pose = copy.deepcopy(msg.pose.pose) output.pose.covariance = cov_mat.tolist() pub_vo = rospy.Publisher('/vo_fixed', Odometry, queue_size=10) pub_vo.publish(output)
def pub_odom(self): self.tf_config() self.current_time = rospy.Time.now() dt = (self.current_time - self.last_time).to_sec() '''if(self.z!=0): delta_th = self.vth * dt self.th = self.th + delta_th''' delta_x = (self.vx * cos(self.th)) * dt # - self.vy * sin(self.th) delta_y = (self.vx * sin(self.th)) * dt # + self.vy * cos(self.th) delta_th = self.vth * dt self.th = self.th + delta_th yaw = self.th * (180 / pi) self.x = self.x + delta_x self.y = self.y + delta_y self.last_time = self.current_time odom_quat = tf.transformations.quaternion_from_euler(0, 0, self.th) # publish odom odom = Odometry() odom.header.stamp = self.current_time odom.header.frame_id = "odom" odom.pose.pose = Pose(Point(self.x, self.y, 0), Quaternion(*odom_quat)) odom.child_frame_id = "base_link" odom.twist.twist = Twist(Vector3(self.vx, 0, 0), Vector3(0, 0, self.vth)) self.odom_pub.publish(odom) # publish tf # rospy.loginfo("pub_tf: " + str(self.x) + ", "+ str(self.y) +", " +str(yaw)) '''self.tf_broadcaster.sendTransform(
def pub_est(self,loc_x,loc_y,loc_z): # pack up estimate to ROS msg and publish """ location=Vector3Stamped() location.header.stamp = rospy.Time.now() location.vector.x = self.xhat[0] location.vector.y = self.xhat[1] location.vector.z = self.xhat[2] self.pub_location.publish(location) """ xhat_odom = Odometry() xhat_odom.header.stamp = rospy.Time.now() xhat_odom.header.frame_id="world" xhat_odom.child_frame_id="world" xhat_odom.pose.pose.position.x = loc_x # pn xhat_odom.pose.pose.position.y = loc_y # pe xhat_odom.pose.pose.position.z = loc_z # pd #quat = tf.transformations.quaternion_from_euler(self.xhat[6].copy(), self.xhat[7].copy(), self.xhat[8].copy()) xhat_odom.pose.pose.orientation.x = 0 xhat_odom.pose.pose.orientation.y = 0 xhat_odom.pose.pose.orientation.z = 0.707107 xhat_odom.pose.pose.orientation.w = 0.707107 #xhat_odom.twist.twist.linear.x = 0 # u #xhat_odom.twist.twist.linear.y = 0 # v #xhat_odom.twist.twist.linear.z = 0 # w estimate_pub_.publish(xhat_odom) pose_temp=PoseStamped() pose_temp.header=xhat_odom.header pose_temp.pose=xhat_odom.pose.pose groundtruth_path.header = pose_temp.header groundtruth_path.poses.append(pose_temp) pub1_.publish(groundtruth_path)
def main(): #r = rospy.Rate(10) while not rospy.is_shutdown(): str = ard.readline() print str list = str.split(' ') if len(list) == 6: try: x = float(list[0]) y = float(list[1]) theta = float(list[2]) v_x = float(list[3]) v_y = float(list[4]) omega = float(list[5]) except ValueError: pass #print x, y, theta, v_x, v_y, omega quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) odom_br.sendTransform((x, y, 0), quaternion, rospy.Time.now(), "odom", "base_link") odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = 0 odom.pose.pose.orientation.x = quaternion[0] odom.pose.pose.orientation.y = quaternion[1] odom.pose.pose.orientation.z = quaternion[2] odom.pose.pose.orientation.w = quaternion[3] odom.child_frame_id = "base_link" odom.twist.twist.linear.x = v_x odom.twist.twist.linear.y = v_y #odom.twist.twist.linear.z = 0 #odom.twist.twist.angular.x = 0 #odom.twist.twist.angular.y odom.twist.twist.angular.z = omega odom_pub.publish(odom)
def cmdvel_to_Odm(v_x,v_th,dt,now_time): global x global y global theta #------------------------- x += v_x * math.cos(theta) * dt y += v_x * math.sin(theta) * dt theta += v_th * dt #------------------------- odm_Quaternion = tf.transformations.quaternion_from_euler(0,0,theta) ##print(odm_Quaternion) odm = Odometry() ##set the header odm.header.stamp = now_time odm.header.frame_id = "odom" ##set the position odm.pose.pose.position.x = x odm.pose.pose.position.y = y odm.pose.pose.position.z = 0.0 ## set the orientation odm.pose.pose.orientation.x = odm_Quaternion[0] odm.pose.pose.orientation.y = odm_Quaternion[1] odm.pose.pose.orientation.z = odm_Quaternion[2] odm.pose.pose.orientation.w = odm_Quaternion[3] ##set the velocity odm.child_frame_id = "base_link" odm.twist.twist.linear.x = v_x odm.twist.twist.angular.z = v_th pubOdm.publish(odm) print("+++++++++ Odmetry ++++++++++") print("x : %f" % x) print("theta : %f" % theta) return 0
def odometry_raw_cb(args): odom = Odometry() x, y, theta = tuple(args) current_time = rospy.get_rostime() odom.header.stamp.secs = current_time.secs odom.header.stamp.nsecs = current_time.nsecs odom.header.frame_id = "map" odom.child_frame_id = "base_link" odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = 0 quat = tf.transformations.quaternion_from_euler(0, 0, theta) odom.pose.pose.orientation.x = quat[0] odom.pose.pose.orientation.y = quat[1] odom.pose.pose.orientation.z = quat[2] odom.pose.pose.orientation.w = quat[3] global pub pub.publish(odom)
def handle_robot_pose(poseMsg, currTime): br = tf.TransformBroadcaster() # create a broadcaster theta = tf.transformations.quaternion_from_euler(0, 0, poseMsg[2]) odom = Odometry() odom.header.stamp = currTime odom.header.frame_id = "odom" #set the position odom.pose.pose.position = Point(poseMsg[0], poseMsg[1], 0) odom.pose.pose.orientation = theta #set the velocity odom.child_frame_id = "base_link" #odom.twist.twist.linear.x = 0 #odom.twist.twist.linear.y = 0 #odom.twist.twist.angular.z = 0 odomPub = rospy.Publisher('odom', Odometry, queue_size=1) odomPub.publish(odom) br.sendTransform((poseMsg[0], poseMsg[1], 0.0), theta, currTime, "base_link", "odom") # publish it on ros #create odometry msg print "transform sent"
def alvarcb(self, markers): rospy.logdebug("Detected markers!") # can we find the correct marker? for m in markers.markers: if m.id == self.marker_id: odom_meas = Odometry() odom_meas.header.frame_id = self.frame_id m.pose.header.frame_id = self.camera_frame_id odom_meas.child_frame_id = self.base_frame_id odom_meas.header.stamp = m.header.stamp m.pose.header.stamp = m.header.stamp # now we need to transform this pose measurement from the camera # frame into the frame that we are reporting measure odometry in pose_transformed = self.transform_pose(m.pose) if pose_transformed is not None: odom_meas.pose.pose = pose_transformed.pose # Now let's add our offsets: odom_meas = odom_conversions.odom_add_offset( odom_meas, self.odom_offset) self.meas_pub.publish(odom_meas) self.send_transforms(odom_meas) self.publish_path(m.pose) return
def write_odometry(ts_ns, T_XB30_XB3k, bag): ros_timestamp = nanoseconds_to_ros_timestamp(ts_ns) T_B0_Bk = get_T_B_XB3() * T_XB30_XB3k * get_T_B_XB3().inverse() rospose = Odometry() rospose.child_frame_id = "B" rospose.header.stamp = ros_timestamp rospose.pose.pose.position.x = T_B0_Bk.getPosition()[0] rospose.pose.pose.position.y = T_B0_Bk.getPosition()[1] rospose.pose.pose.position.z = T_B0_Bk.getPosition()[2] rospose.pose.pose.orientation.x = T_B0_Bk.getRotation().x() rospose.pose.pose.orientation.y = T_B0_Bk.getRotation().y() rospose.pose.pose.orientation.z = T_B0_Bk.getRotation().z() rospose.pose.pose.orientation.w = T_B0_Bk.getRotation().w() rospose.twist.twist.linear.x = 0.0 rospose.twist.twist.linear.y = 0.0 rospose.twist.twist.linear.z = 0.0 rospose.twist.twist.angular.x = 0.0 rospose.twist.twist.angular.y = 0.0 rospose.twist.twist.angular.z = 0.0 bag.write('/visual_odometry', rospose, t=ros_timestamp)
def _link_states_cb(self, msg): """ :param msg: :type msg: LinkStates :return: """ if self._sphere_link_name is None: for i, name in enumerate(msg.name): if "sphere" in name.lower(): self._sphere_link_name = name self._sphere_link_index = i break else: odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = "odom" odom.child_frame_id = "sphere" odom.pose.pose = msg.pose[self._sphere_link_index] odom.pose.covariance = [0]*36 odom.twist.twist = msg.twist[self._sphere_link_index] odom.twist.covariance = [0]*36 self.odometry_publisher.publish(odom)
def save_odom_bag(seq, seconds, nanoseconds, v_x, v_y, v_ang, x, y, orientation): odom_msg = Odometry() odom_msg.header.seq = seq odom_msg.header.stamp.secs = seconds odom_msg.header.stamp.nsecs = nanoseconds odom_msg.header.frame_id = "odom" odom_topic = "/odom" odom_msg.child_frame_id = "base_link" odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y odom_msg.pose.pose.position.z = 0 odom_msg.pose.pose.orientation.x = orientation[0] odom_msg.pose.pose.orientation.y = orientation[1] odom_msg.pose.pose.orientation.z = orientation[2] odom_msg.pose.pose.orientation.w = orientation[3] odom_msg.twist.twist.linear.x = v_x odom_msg.twist.twist.linear.y = v_y 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 = v_ang bag.write(odom_topic, odom_msg, odom_msg.header.stamp)
def publish_se2(self, event): if self.current_x and self.current_y and self.current_yaw and self.okay_to_publish: se2 = SE2() se2.x = self.current_x se2.y = self.current_y se2.yaw = self.current_yaw self.se2_pub.publish(se2) odom = Odometry() header = Header() header.stamp = rospy.Time.now() header.frame_id = "odom" odom.header = header odom.child_frame_id = "base_link" pose = PoseWithCovariance() pose.pose.position.x = self.current_x pose.pose.position.y = self.current_y pose.pose.orientation = self.current_orientation odom.pose = pose self.odom_pub.publish(odom)
def create_odometry_message(world_x, world_y, world_theta, world_x_linear_v, world_y_linear_v, world_z_angular_v, odom_time, base_frame_id, world_frame_id): # Convert world orientation (theta) to a Quaternion for use with tf and Odometry quat_vals = tf.transformations.quaternion_from_euler(0, 0, world_theta) quat = Quaternion() quat.x = quat_vals[0] quat.y = quat_vals[1] quat.z = quat_vals[2] quat.w = quat_vals[3] odom = Odometry() odom.header.stamp = odom_time odom.header.frame_id = world_frame_id odom.pose.pose.position.x = world_x odom.pose.pose.position.y = world_y odom.pose.pose.position.z = 0.0 # Because this robot can't fly to a vertical position odom.pose.pose.orientation = quat odom.child_frame_id = base_frame_id odom.twist.twist.linear.x = world_x_linear_v odom.twist.twist.linear.y = world_y_linear_v odom.twist.twist.angular.z = world_z_angular_v return odom
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth): # quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta) quat = euler2quaternion(0, 0, cur_theta) current_time = rospy.Time.now() odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = 'odom' odom.pose.pose.position.x = cur_x odom.pose.pose.position.y = cur_y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = Quaternion(*quat) odom.pose.covariance[0] = 0.01 odom.pose.covariance[7] = 0.01 odom.pose.covariance[14] = 99999 odom.pose.covariance[21] = 99999 odom.pose.covariance[28] = 99999 odom.pose.covariance[35] = 0.01 odom.child_frame_id = 'base_link' odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth odom.twist.covariance = odom.pose.covariance self.ekf_pub.publish(odom)
def create_odometry(self, stamp, robot, velocity_x, velocity_theta): odom = Odometry() odom.header.stamp = stamp odom.header.frame_id = self._odom_frame_id q = quaternion_from_euler(0, 0, robot.theta) #set the position odom.pose.pose.position.x = robot.x odom.pose.pose.position.y = robot.y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] #set the velocity odom.child_frame_id = self._base_frame_id odom.twist.twist.linear.x = velocity_x 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 = velocity_theta return odom
def gxPose2d_to_odom(msg,t,outbag,frame_id,child_frame_id,offset): ''' turn /gx/RosPose2d topic to /odom topic change the type of gx_sensor/RosPose2d to nav_msgs/Odometry /gx/RosPose2d topic message example: timestamp: 1562430246571000 x: 2.03333210945 y: -0.198542118073 theta: 0.551429688931 linearVelocity: 0.0 angularVelociy: -0.00757710635662 the topic is in base_link coordinate ''' str_gx=[x.split(":") for x in str(msg).split("\n")] unit_odom=Odometry() unit_odom.pose.pose.position=Point(float(str_gx[1][1]),float(str_gx[2][1]),0) unit_odom.pose.pose.orientation=Quaternion(*tf.transformations.quaternion_from_euler(0,0,float(str_gx[3][1]))) unit_odom.header.stamp=Time.from_sec(gx_msg_stamp(msg).to_sec() + offset) unit_odom.header.frame_id=frame_id unit_odom.child_frame_id=child_frame_id unit_odom.twist.twist.angular=Point(0,0,float(str_gx[5][1])) unit_odom.twist.twist.linear=Point(float(str_gx[4][1])*math.cos(float(str_gx[3][1])),float(str_gx[4][1])*math.sin(float(str_gx[3][1])),0) outbag.write('/odom',unit_odom,t)
def publish_pose(self, pose, twist, child_frame='base_link'): orientation = [ pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w ] position = [ pose.pose.position.x, pose.pose.position.y, pose.pose.position.z ] rospy.loginfo_throttle(1.0, "Calculated pose: {}".format(pose)) self.tfb.sendTransform(position, orientation, pose.header.stamp, child_frame, pose.header.frame_id) odom = Odometry() odom.header = pose.header odom.pose = PoseWithCovariance(pose=pose.pose) odom.pose.covariance[0] = -1 odom.twist = TwistWithCovariance(twist=twist.twist) odom.twist.covariance[0] = -1 odom.child_frame_id = child_frame self.odom_pub.publish(odom)
def parse_line(line): odom_data = json.loads(line) if not odom_data: return None msg = Odometry() msg.header.stamp.secs = int(odom_data["secs"]) msg.header.stamp.nsecs = int(odom_data["tf_nsecs"]) msg.header.frame_id = "map" msg.child_frame_id = "map" msg.twist.twist.linear.x = float(odom_data["v"]) msg.twist.twist.angular.z = float(odom_data["w"]) msg.pose.pose.position = Point(float(odom_data["px"]), float(odom_data["py"]), float(odom_data["pz"])) msg.pose.pose.orientation = Quaternion(float(odom_data["ox"]), float(odom_data["oy"]), float(odom_data["oz"]), float(odom_data["ow"])) pose = PoseStamped() pose.header = msg.header pose.pose = msg.pose.pose return msg, pose
def timer_callback(self, event): if self.last_recieved_stamp is None: return if self.flag_reading is True: return cmd = Odometry() cmd.header.stamp = self.last_recieved_stamp cmd.header.frame_id = self.odom_frame cmd.child_frame_id = self.base_frame cmd.pose.pose = self.last_received_pose cmd.twist.twist = self.last_received_twist self.odomPublisher.publish(cmd) if self.publish_tf: tf = TransformStamped(header=Header(frame_id=cmd.header.frame_id, stamp=cmd.header.stamp), child_frame_id=cmd.child_frame_id, transform=Transform( translation=cmd.pose.pose.position, rotation=cmd.pose.pose.orientation)) self.tfBroadcaster.sendTransform(tf)
def publish_odometry(self, translation, quaternion, child, parent): odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = parent odom.child_frame_id = child odom.header.seq = self.seq odom.pose.pose.position.x = translation[0] odom.pose.pose.position.y = translation[1] odom.pose.pose.position.z = translation[2] quat = Quaternion() quat.x = quaternion[0] quat.y = quaternion[1] quat.z = quaternion[2] quat.w = quaternion[3] odom.pose.pose.orientation = quat self.pub_odom.publish(odom) self.seq += 1
def publish_odometry(x, y, z, th, t, prev): global odom_pub odom = Odometry() time_diff = (t.to_sec() - prev[4].to_sec()) odom.header.frame_id = "odom" odom.child_frame_id = "base_link" odom.header.stamp = t odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = z q = tf.transformations.quaternion_from_euler(0, 0, th) odom.pose.pose.orientation.x = q[0] odom.pose.pose.orientation.y = q[1] odom.pose.pose.orientation.z = q[2] odom.pose.pose.orientation.w = q[3] odom.twist.twist.linear.x = (x - prev[0]) / time_diff odom.twist.twist.linear.y = (y - prev[1]) / time_diff odom.twist.twist.linear.z = (z - prev[2]) / time_diff odom.twist.twist.angular.z = (th - prev[3]) / time_diff odom_pub.publish(odom)
def metadata_to_odom(metadata, timestamp, frame_id, child_frame_id): """ Converts a metadata dictionary to a ROS odometry message. Args: metadata: A dictionary containing agent metadata parsed from Unity AND pre-processed to be converted to the correct frame. timestamp: A rospy.Time instance for the ROS Odom message instance. frame_id: A string representing the reference frame (world frame). Returns: An Odom ROS message instance that can immediately be published. """ odom = Odometry() odom.header.stamp = timestamp odom.header.frame_id = frame_id odom.child_frame_id = child_frame_id # Pose is in the ENU world frame. odom.pose.pose.position.x = metadata['position'][0] odom.pose.pose.position.y = metadata['position'][1] odom.pose.pose.position.z = metadata['position'][2] odom.pose.pose.orientation.x = metadata['quaternion'][0] odom.pose.pose.orientation.y = metadata['quaternion'][1] odom.pose.pose.orientation.z = metadata['quaternion'][2] odom.pose.pose.orientation.w = metadata['quaternion'][3] # Twist is in the body frame (camera/imu). odom.twist.twist.linear.x = metadata['velocity'][0] odom.twist.twist.linear.y = metadata['velocity'][1] odom.twist.twist.linear.z = metadata['velocity'][2] odom.twist.twist.angular.x = metadata['ang_vel'][0] odom.twist.twist.angular.y = metadata['ang_vel'][1] odom.twist.twist.angular.z = metadata['ang_vel'][2] return odom
def write_gt(gt, i, bag): utime = gt[i, 0] x = gt[i, 1] y = gt[i, 2] z = gt[i, 3] euler_roll_rad = gt[i, 4] euler_pitch_rad = gt[i, 5] euler_yaw_rad = gt[i, 6] T_GT_BodyZDown = getTransformationFromEulerAnglesRollPitchYawRadXYZMeters( euler_roll_rad, euler_pitch_rad, euler_yaw_rad, x, y, z) T_GT_BZup = T_GT_BodyZDown * get_T_BZU_B().inverse() timestamp = microseconds_to_ros_timestamp(utime) rospose = Odometry() rospose.child_frame_id = "Ground-Truth" rospose.header.stamp = timestamp rospose.pose.pose.position.x = T_GT_BZup.getPosition()[0] rospose.pose.pose.position.y = T_GT_BZup.getPosition()[1] rospose.pose.pose.position.z = T_GT_BZup.getPosition()[2] rospose.pose.pose.orientation.x = T_GT_BZup.getRotation().x() rospose.pose.pose.orientation.y = T_GT_BZup.getRotation().y() rospose.pose.pose.orientation.z = T_GT_BZup.getRotation().z() rospose.pose.pose.orientation.w = T_GT_BZup.getRotation().w() rospose.twist.twist.linear.x = 0.0 rospose.twist.twist.linear.y = 0.0 rospose.twist.twist.linear.z = 0.0 rospose.twist.twist.angular.x = 0.0 rospose.twist.twist.angular.y = 0.0 rospose.twist.twist.angular.z = 0.0 bag.write('/ground_truth', rospose, t=timestamp)
def main(): rospy.init_node('odometry_publisher') coord = Coord(0.0, 0.0, 0.0) odom_pub = rospy.Publisher("odom", Odometry, queue_size=50) odom_broadcaster = tf.TransformBroadcaster() rospy.Subscriber("moves", String, callback, coord) while not rospy.is_shutdown(): current_time = rospy.Time.now() # since all odometry is 6DOF we'll need a quaternion created from yaw odom_quat = tf.transformations.quaternion_from_euler(0, 0, coord.th) # first, we'll publish the transform over tf odom_broadcaster.sendTransform((coord.x, coord.y, 0.), odom_quat, current_time, "base_link", "odom") print(coord.x) # next, we'll publish the odometry message over ROS odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = "odom" # set the position odom.pose.pose = Pose(Point(coord.x, coord.y, 0.), Quaternion(*odom_quat)) # set the velocity odom.child_frame_id = "base_link" odom.twist.twist = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) #Velocities =0 # publish the message odom_pub.publish(odom) pass
def publish_transform_pos(self, msg): if self.indoor: # Angle in rad (roll, pitch, yaw_before) = tf.transformations.euler_from_quaternion([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) # Transform the camera data for Rviz # 468655 # 5264080 # 468598.24 # 5264012.01 x, y, yaw = self.transformFromLastGPS([ msg.pose.pose.position.x, msg.pose.pose.position.y, yaw_before ], [ 468655.0 + self.last_robot_x_edit, 5264080.0 + self.last_robot_y_edit ]) # x, y, yaw = self.transformFromLastGPS([msg.pose.pose.position.x, # msg.pose.pose.position.y, yaw_before], [468655.0+3.0, 5264080.0-1.0]) odom = Odometry() odom.header.stamp = rospy.Time.now() odom.header.frame_id = 'odom' odom.child_frame_id = 'base_link' odom.pose.pose.position.x = x odom.pose.pose.position.y = y odom.pose.pose.position.z = 0 # Don't forget the "Quaternion()" and * odom.pose.pose.orientation = Quaternion( *tf.transformations.quaternion_from_euler(0, 0, yaw)) # publisher self.transform_cameraPos_pub.publish(odom)
def get_relative(self, odometry): pose = odometry.pose x = pose.pose.position.x y = pose.pose.position.y orientation = pose.pose.orientation quaternion = PyKDL.Rotation.Quaternion(orientation.x, orientation.y, orientation.z, orientation.w) if not self.initial_quaternion: self.initial_quaternion = quaternion self.initial_x = x self.initial_y = x else: relative_x = x - self.initial_x relative_y = y - self.initial_y relative_quaternion = self.initial_quaternion * quaternion.Inverse( ) relative_quaternion = relative_quaternion.GetQuaternion() odometry = Odometry() header = Header() header.stamp = rospy.Time.now() header.frame_id = "odom" odometry.header = header odometry.child_frame_id = "base_link" odometry.pose.pose.position.x = relative_x odometry.pose.pose.position.y = relative_y orientation = Quaternion() orientation.x = relative_quaternion[0] orientation.y = relative_quaternion[1] orientation.z = relative_quaternion[2] orientation.w = relative_quaternion[3] odometry.pose.pose.orientation = orientation self.relative_pub.publish(odometry)
def publish_odom(self, cur_x, cur_y, cur_theta, vx, vth): """Publish odometry message""" quat = tf.transformations.quaternion_from_euler(0, 0, cur_theta) current_time = rospy.Time.now() # TODO: why was this commented out? #br = tf.TransformBroadcaster() #br.sendTransform((cur_x, cur_y, 0), # tf.transformations.quaternion_from_euler(0, 0, cur_theta), # current_time, # "base_link", # "odom") odom = Odometry() odom.header.stamp = current_time odom.header.frame_id = 'odom' odom.pose.pose.position.x = cur_x odom.pose.pose.position.y = cur_y odom.pose.pose.position.z = 0.0 odom.pose.pose.orientation = Quaternion(*quat) # TODO: Why are the covariances set at these values? Do they even make sense? odom.pose.covariance[0] = 0.01 odom.pose.covariance[7] = 0.01 odom.pose.covariance[14] = 99999 odom.pose.covariance[21] = 99999 odom.pose.covariance[28] = 99999 odom.pose.covariance[35] = 0.01 odom.child_frame_id = 'base_link' odom.twist.twist.linear.x = vx odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth odom.twist.covariance = odom.pose.covariance self.odom_pub.publish(odom)