diff --git a/robomaster_pi/connection.py b/robomaster_pi/connection.py
new file mode 100644
index 0000000000000000000000000000000000000000..8c76cab32dc9cf70604fdba133a0e15e0c803ce9
--- /dev/null
+++ b/robomaster_pi/connection.py
@@ -0,0 +1,89 @@
+import robomaster
+from robomaster import robot
+import math
+import time
+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
+
+
+from rclpy.node import Node
+x, y, z = 0, 0, 0
+
+
+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
+
+def sub_position_handler(position_info):
+ tf_broadcaster = TransformBroadcaster()
+ t = TransformStamped()
+ t.header.stamp = 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)
+
+
+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):
+
+ if(msg.angular.z == 0.0 and msg.linear.x == 0.0 and msg.linear.y == 0.0):
+ ep_chassis.drive_speed(x = 0, y = 0, z = 0.5)
+ else:
+ ep_chassis.drive_speed(x = msg.linear.x, y = msg.linear.y, z = msg.angular.z)
+
+
+if __name__ == '__main__':
+ ep_robot = robot.Robot()
+ ep_robot.initialize(conn_type="rndis")
+
+ ep_chassis = ep_robot.chassis
+ #ep_chassis.sub_position(freq=10, callback=sub_position_handler)
+
+ rclpy.init()
+ node = vel_from_ros_node()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+ ep_robot.close()
+
\ No newline at end of file
diff --git a/robomaster_pi/robot_ros_odom_wifi.py b/robomaster_pi/robot_ros_odom_wifi.py
deleted file mode 100644
index 45db68205ae608adf01f7d313a6f5bc7558fc0c4..0000000000000000000000000000000000000000
--- a/robomaster_pi/robot_ros_odom_wifi.py
+++ /dev/null
@@ -1,77 +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 tf2_ros import TransformBroadcaster
-
-
-from rclpy.node import Node
-
-host = "192.168.1.128"
-port = 40923
-address = (host, int(port))
-s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
-s.connect(address)
-s.send(("command;").encode('utf-8'))
-abfangen = s.recv(1024)
-
-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)
- s.send(("chassis position ?;").encode('utf-8'))
- buf = s.recv(1024)
- 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_footprint"
- 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.5)
-
-def main():
- rclpy.init()
- node = Robomaster_odom()
-
- rclpy.spin(node)
- rclpy.shutdown()
\ No newline at end of file
diff --git a/robomaster_pi/robot_ros_vel_wifi.py b/robomaster_pi/robot_ros_vel_wifi.py
deleted file mode 100644
index 8ce573d2f141497d42d49c759eefa64057e20b72..0000000000000000000000000000000000000000
--- a/robomaster_pi/robot_ros_vel_wifi.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.1.128"
-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
diff --git a/setup.py b/setup.py
index 4b1276618662ceba75cfadfbff31f74011a51428..0cabf1f31b876b558a39643f30368dd87eca8a13 100644
--- a/setup.py
+++ b/setup.py
@@ -20,10 +20,9 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
+ "connection = robomaster_pi.connection:main"
"robot_ros_vel = robomaster_pi.robot_ros_vel:main",
"robot_ros_odom = robomaster_pi.robot_ros_odom:main",
- "robot_ros_vel_wifi = robomaster_pi.robot_ros_vel_wifi:main",
- "robot_ros_odom_wifi = robomaster_pi.robot_ros_odom_wifi:main"
],
},
)