From 3633e678da4f55bb7d81bf21f41b657a208c6723 Mon Sep 17 00:00:00 2001
From: eharlan <eharlan@uni-bremen.de>
Date: Thu, 11 May 2023 13:02:46 +0200
Subject: [PATCH] a
---
robomaster_pi/robot_ros_odom.py | 78 ---------------------------------
robomaster_pi/robot_ros_vel.py | 42 ------------------
2 files changed, 120 deletions(-)
delete mode 100644 robomaster_pi/robot_ros_odom.py
delete mode 100644 robomaster_pi/robot_ros_vel.py
diff --git a/robomaster_pi/robot_ros_odom.py b/robomaster_pi/robot_ros_odom.py
deleted file mode 100644
index ffc35d6..0000000
--- a/robomaster_pi/robot_ros_odom.py
+++ /dev/null
@@ -1,78 +0,0 @@
-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()
diff --git a/robomaster_pi/robot_ros_vel.py b/robomaster_pi/robot_ros_vel.py
deleted file mode 100644
index 52175c1..0000000
--- a/robomaster_pi/robot_ros_vel.py
+++ /dev/null
@@ -1,42 +0,0 @@
-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
--
GitLab