- i2c_write_reg(0x50, 0x1, struct.pack(">hhhh", right, right, left, left))
+ # 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.position.x = posx
+ odom.pose.pose.position.y = posy
+ odom.pose.pose.position.z = 0.0
+ odom.pose.pose.orientation.x = odom_quat[0]
+ odom.pose.pose.orientation.y = odom_quat[1]
+ odom.pose.pose.orientation.z = odom_quat[2]
+ odom.pose.pose.orientation.w = odom_quat[3]
+
+ # set the velocity
+ odom.child_frame_id = "base_link"
+ odom.twist.twist.linear.x = speed_trans
+ odom.twist.twist.linear.y = 0.0
+ odom.twist.twist.angular.z = speed_rot
+
+ # publish the message
+ self.pub_odom.publish(odom)
+
+
+ def set_speed(self, trans, rot):
+ i2c_write_reg(0x50, 0x50, struct.pack(">ff", trans, rot))