diff --git a/robomaster_pi/__pycache__/conn_odom.cpython-38.pyc b/robomaster_pi/__pycache__/conn_odom.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4fe185aeeeace4ac346957341d9d85f6aa7b8357
Binary files /dev/null and b/robomaster_pi/__pycache__/conn_odom.cpython-38.pyc differ
diff --git a/robomaster_pi/__pycache__/connection.cpython-38.pyc b/robomaster_pi/__pycache__/connection.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..91b94c78b37487969761e2ff0570898e1485dfe1
Binary files /dev/null and b/robomaster_pi/__pycache__/connection.cpython-38.pyc differ
diff --git a/robomaster_pi/__pycache__/robot_ros_odom.cpython-38.pyc b/robomaster_pi/__pycache__/robot_ros_odom.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..760245cbfb7632722b06e3d96968b1b219c6a731
Binary files /dev/null and b/robomaster_pi/__pycache__/robot_ros_odom.cpython-38.pyc differ
diff --git a/robomaster_pi/conn_odom.py b/robomaster_pi/conn_odom.py
index 7fe1a2d099a3be6136c035ad7b40de025339d163..b5616d63d1a22523798516c34a0de2ce8db02bb8 100644
--- a/robomaster_pi/conn_odom.py
+++ b/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
diff --git a/robomaster_pi/robot_ros_odom.py b/robomaster_pi/robot_ros_odom.py
index c68e32358b8e9b05ed90b9ad24bae910786620d0..ffc35d6d8ee4777ab233170d2423100851a3b8bf 100644
--- a/robomaster_pi/robot_ros_odom.py
+++ b/robomaster_pi/robot_ros_odom.py
@@ -1,19 +1,25 @@
-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