update

robomaster_pi/conn_odom.py

@@ -5,39 +5,82 @@ import math
 import numpy as np
 import robomaster
 from robomaster import robot
+import tf2_ros
+import tf2_geometry_msgs
+from geometry_msgs.msg import TransformStamped, Quaternion
 
 class OdometryPublisher(Node):
+    zWinkel = 0.0
     
     def __init__(self):
         super().__init__('odometry_publisher')
         self.publisher_ = self.create_publisher(Odometry, 'odometry', 10)
+        self.tf_broadcaster_ = tf2_ros.StaticTransformBroadcaster(self)
+        self.transform_stamped_ = TransformStamped()
+        self.transform_stamped_.header.frame_id = "odom"
+        self.transform_stamped_.child_frame_id = "base_footprint"
         ep_robot = robot.Robot()
         ep_robot.initialize(conn_type="rndis")
+    
         ep_chassis = ep_robot.chassis
-        ep_chassis.sub_position(freq=10, callback=self.sub_position_handler)
-        self.x = 0
-        self.y = 0
-        self.z = 0
+        ep_chassis.sub_position(freq=50, callback=self.sub_position_handler)
+        ep_chassis.sub_imu(freq=50, callback=self.sub_imu_info_handler)
+        
+
+    def sub_imu_info_handler(self, imu_info):
+        acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = imu_info
+        if(self.zWinkel + (gyro_z /50)* (180 / math.pi) < 195 and self.zWinkel + (gyro_z /50)* (180 / math.pi) >= -165):
+            self.zWinkel = self.zWinkel + (gyro_z /50)* (180 / math.pi)
+        else:
+            if(self.zWinkel + (gyro_z /50)* (180 / math.pi) > 195):
+                self.zWinkel = self.zWinkel + (gyro_z /50)* (180 / math.pi) - 360
+            if(self.zWinkel + (gyro_z /50)* (180 / math.pi) <= -165):
+                self.zWinkel = self.zWinkel + (gyro_z /50)* (180 / math.pi) + 360
+        
+
+            
+        
+
+        
 
     def sub_position_handler(self, position_info):
         self.x, self.y, self.z = position_info
-        print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.z))
+        print("chassis position: x:{0}, y:{1}, z:{2}".format(self.x, self.y, self.zWinkel))
         self.publish_odometry()
+        self.publish_transform()
+ 
 
     def publish_odometry(self):
-        print("ja")
-        q = self.quaternion_from_euler(0, 0, -self.z* math.pi/180)
+        
+        q = self.quaternion_from_euler(0, 0, -self.zWinkel / (180 / math.pi))
         msg = Odometry()
         msg.header.stamp = self.get_clock().now().to_msg()
-        msg.pose.pose.position.x = self.x
+        msg.pose.pose.position.x = -self.x
         msg.pose.pose.position.y = self.y
-        msg.pose.pose.position.z = 0
-        msg.pose.pose.orientation.x = 0
-        msg.pose.pose.orientation.y = 0
+        msg.pose.pose.position.z = 0.0
+        msg.pose.pose.orientation.x = 0.0
+        msg.pose.pose.orientation.y = 0.0
         msg.pose.pose.orientation.z = q[2]
-        msg.pose.pose.orientation.w = 0
+        msg.pose.pose.orientation.w = 0.0
+        
         self.publisher_.publish(msg)
 
+    def publish_transform(self):
+        self.transform_stamped_.header.stamp = self.get_clock().now().to_msg()
+        self.transform_stamped_.transform.translation.x = -self.x
+        self.transform_stamped_.transform.translation.y = self.y
+        self.transform_stamped_.transform.translation.z = 0.0
+        q = self.quaternion_from_euler(0.0, 0.0, -self.zWinkel / (180 / math.pi))
+        
+        self.transform_stamped_.transform.rotation.x = q[0]
+        self.transform_stamped_.transform.rotation.y = q[1]
+        self.transform_stamped_.transform.rotation.z = q[2]
+        self.transform_stamped_.transform.rotation.w = q[3]
+
+        
+        self.tf_broadcaster_.sendTransform(self.transform_stamped_)
+        
+
     def quaternion_from_euler(self, ai, aj, ak):
         ai /= 2.0
         aj /= 2.0

robomaster_pi/robot_ros_odom.py

-import robomaster
-from robomaster import robot
 import math
 import time
+from geometry_msgs.msg import TransformStamped
+
 import numpy as np
 import rclpy
 from rclpy.node import Node
 from tf2_ros import TransformBroadcaster
-from geometry_msgs.msg import TransformStamped
-from geometry_msgs.msg import Twist
-from tf2_ros import TransformBroadcaster
 
 
 from rclpy.node import Node
-x, y, z = 0, 0, 0
 
+ser = serial.Serial()
+ser.port = '/dev/ttyS0'
+ser.baudrate = 115200
+ser.bytesize = serial.EIGHTBITS
+ser.stopbits = serial.STOPBITS_ONE
+ser.parity = serial.PARITY_NONE
+ser.timeout = 0.2
+ser.open()
+ser.write(("command;").encode('utf-8'))
+leer = ser.readall()
 
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
@@ -39,62 +45,34 @@ def quaternion_from_euler(ai, aj, ak):
     return q
 
 class Robomaster_odom(Node):
+    def __init__(self):
 
-    def sub_position_handler(position_info):
-        print("handler")
-        tf_broadcaster = TransformBroadcaster()
-        t = TransformStamped()
-        t.header.stampep_chassis = time.get_clock().now().to_msg()
-        t.header.frame_id = 'odom'
-        t.child_frame_id = "base_footprint"
-        t.transform.translation.x = position_info.x
-        t.transform.translation.y = -position_info.y
-        t.transform.translation.z = 0.0
-        q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180)
-        t.transform.rotation.x = q[0]
-        t.transform.rotation.y = q[1]
-        t.transform.rotation.z = q[2]
-        t.transform.rotation.w = q[3]
-        tf_broadcaster.sendTransform(t)
-        print("ok")
-
-    def __init__(self, ep_chassis):
-        super().__init__("robomaster_odom")
-        
-        def sub_position_handler(position_info):
-            print("handler")
-            tf_broadcaster = TransformBroadcaster()
+        while True:
+            super().__init__("robomaster_odom")
+            self.tf_broadcaster = TransformBroadcaster(self)
+            ser.write(("chassis position ?;").encode('utf-8'))
+            buf = ser.readall()
+            xyz = buf.decode('utf-8')
+            self.get_logger().info(xyz)
+            xyzlist = xyz.split(" ")
             t = TransformStamped()
-            t.header.stampep_chassis = time.get_clock().now().to_msg()
+            t.header.stamp = self.get_clock().now().to_msg()
             t.header.frame_id = 'odom'
-            t.child_frame_id = "base_footprint"
-            t.transform.translation.x = position_info.x
-            t.transform.translation.y = -position_info.y
+            t.child_frame_id = "base_link"
+            t.transform.translation.x = float(xyzlist[0])
+            t.transform.translation.y = float(xyzlist[1])
             t.transform.translation.z = 0.0
-            q = quaternion_from_euler(0, 0, -position_info.z* math.pi/180)
+            q = quaternion_from_euler(0, 0, float(xyzlist[2])* math.pi/180)
             t.transform.rotation.x = q[0]
             t.transform.rotation.y = q[1]
             t.transform.rotation.z = q[2]
             t.transform.rotation.w = q[3]
-            tf_broadcaster.sendTransform(t)
-            print("ok")
-        
-        ep_chassis.sub_position(freq=10, callback=sub_position_handler)
-        print("sub")
-    
-    
-
+            self.tf_broadcaster.sendTransform(t)
+            time.sleep(0.1)
 
 def main():
-    print("jo")
-    ep_robot = robot.Robot()
-    ep_robot.initialize(conn_type="rndis")
-
-    ep_chassis = ep_robot.chassis
-
     rclpy.init()
-    node = Robomaster_odom(ep_chassis)
+    node = Robomaster_odom()
+    
     rclpy.spin(node)
     rclpy.shutdown()
-
-    ep_robot.close()
\ No newline at end of file