a

robomaster_pi/robot_ros_odom.py

-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 rclpy.node import Node
-
-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
-    aj /= 2.0
-    ak /= 2.0
-    ci = math.cos(ai)
-    si = math.sin(ai)
-    cj = math.cos(aj)
-    sj = math.sin(aj)
-    ck = math.cos(ak)
-    sk = math.sin(ak)
-    cc = ci*ck
-    cs = ci*sk
-    sc = si*ck
-    ss = si*sk
-
-    q = np.empty((4, ))
-    q[0] = cj*sc - sj*cs
-    q[1] = cj*ss + sj*cc
-    q[2] = cj*cs - sj*sc
-    q[3] = cj*cc + sj*ss
-
-    return q
-
-class Robomaster_odom(Node):
-    def __init__(self):
-
-        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.stamp = self.get_clock().now().to_msg()
-            t.header.frame_id = 'odom'
-            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, 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]
-            self.tf_broadcaster.sendTransform(t)
-            time.sleep(0.1)
-
-def main():
-    rclpy.init()
-    node = Robomaster_odom()
-    
-    rclpy.spin(node)
-    rclpy.shutdown()

robomaster_pi/robot_ros_vel.py

-import math
-import time
-import socket
-import sys
-from geometry_msgs.msg import TransformStamped
-
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from geometry_msgs.msg import Twist
-from tf2_ros import TransformBroadcaster
-
-
-from rclpy.node import Node
-
-host = "192.168.42.2"
-port = 40923
-address = (host, int(port))
-s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-s.connect(address)
-s.send(("command;").encode('utf-8'))
-
-
-class vel_from_ros_node(Node):
-    
-    def __init__(self):
-        super().__init__("vel_from_ros")
-        self.get_vel_from_ros_ = self.create_subscription(
-            Twist, "/cmd_vel", self.pose_callback, 10)
-    
-    def pose_callback(self, msg: Twist):
-        command = "chassis speed x " + str(msg.linear.x) + " y " + str(msg.linear.y) + " z " + str(msg.angular.z) + ";"
-        s.send(command.encode('utf-8'))
-
-            
-            
-
-def main():
-    rclpy.init()
-    node2 = vel_from_ros_node()
-    rclpy.spin(node2)
-    rclpy.shutdown()
\ No newline at end of file