From bad871c47d2efc985ffefa1de48cb6482b4c9d97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Fri, 8 Feb 2019 20:07:33 +0100 Subject: [PATCH] move wheel power conversion from tr_control to tr_hat_bridge --- tr_control/README.md | 4 ++-- tr_control/src/tr_control/controller.py | 14 +++----------- tr_control/src/tr_control/main.py | 2 +- tr_control/src/tr_control/robot.py | 8 ++++---- tr_hat_bridge/README.md | 7 +++---- tr_hat_bridge/src/tr_hat_bridge/bridge.py | 14 +++++++++++--- tr_hat_msgs/CMakeLists.txt | 2 +- tr_hat_msgs/msg/MotorPayload.msg | 2 -- tr_hat_msgs/msg/MotorPower.msg | 3 +++ 9 files changed, 28 insertions(+), 28 deletions(-) delete mode 100644 tr_hat_msgs/msg/MotorPayload.msg create mode 100644 tr_hat_msgs/msg/MotorPower.msg diff --git a/tr_control/README.md b/tr_control/README.md index 52b26d9..0aa9da4 100644 --- a/tr_control/README.md +++ b/tr_control/README.md @@ -19,7 +19,7 @@ This node takes velocity commands as Twist messages and communicates with [tr_ha #### Published Topics -* **`tr_hat_bridge/motors`** ([tr_hat_msgs/MotorPayload]) +* **`tr_hat_bridge/motors`** ([tr_hat_msgs/MotorPower]) Motor power values sent to Turtle Hat @@ -54,4 +54,4 @@ This node takes velocity commands as Twist messages and communicates with [tr_ha [tr_hat_bridge]: https://github.com/TurtleRover/tr_ros/tree/master/tr_hat_bridge [geometry_msgs/Twist]: http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html [geometry_msgs/TwistStamped]: http://docs.ros.org/api/geometry_msgs/html/msg/TwistStamped.html -[tr_hat_msgs/MotorPayload]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/msg/MotorPayload.msg \ No newline at end of file +[tr_hat_msgs/MotorPower]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/msg/MotorPower.msg \ No newline at end of file diff --git a/tr_control/src/tr_control/controller.py b/tr_control/src/tr_control/controller.py index 2e8aa39..8038113 100644 --- a/tr_control/src/tr_control/controller.py +++ b/tr_control/src/tr_control/controller.py @@ -5,7 +5,7 @@ class Controller(object): def __init__(self, max_wheel_speed=6.0, input_timeout=0.2): - self.payload = [0, 0, 0, 0] # RR, RL, FR, FL + self.power = [0, 0, 0, 0] # RR, RL, FR, FL self.max_wheel_speed = max_wheel_speed self.check_input = input_timeout > 0.0 self.input_timeout = rospy.Duration(input_timeout) @@ -17,13 +17,6 @@ def __init__(self, max_wheel_speed=6.0, input_timeout=0.2): self.check_input_thread.daemon = True self.check_input_thread.start() - @staticmethod - def convert_motor_power_to_payload(power): - value = int(round(power * 0x7F)) - if value < 0: - value = -value + 0x7F - return value - def check_input_loop(self): while not rospy.is_shutdown(): self.input_received.wait() @@ -33,14 +26,13 @@ def check_input_loop(self): rospy.logwarn(("No input received for more than a specified timeout! " "Stopping the motors")) - self.payload = [0, 0, 0, 0] + self.power = [0, 0, 0, 0] self.input_received.clear() def set_wheels(self, wheel_speeds): for i, wheel_speed in enumerate(wheel_speeds): wheel_speed = min(self.max_wheel_speed, max(-self.max_wheel_speed, wheel_speed)) - motor_power = wheel_speed / self.max_wheel_speed - self.payload[i] = Controller.convert_motor_power_to_payload(motor_power) + self.power[i] = wheel_speed / self.max_wheel_speed if self.check_input: self.last_update = rospy.get_rostime() diff --git a/tr_control/src/tr_control/main.py b/tr_control/src/tr_control/main.py index 35378fd..9d6b4fb 100644 --- a/tr_control/src/tr_control/main.py +++ b/tr_control/src/tr_control/main.py @@ -10,7 +10,7 @@ def main(): r = Robot() while not rospy.is_shutdown(): - r.send_payload() + r.publish_motors() rate.sleep() diff --git a/tr_control/src/tr_control/robot.py b/tr_control/src/tr_control/robot.py index abeedfc..c7d4e28 100644 --- a/tr_control/src/tr_control/robot.py +++ b/tr_control/src/tr_control/robot.py @@ -1,7 +1,7 @@ import rospy from geometry_msgs.msg import Twist, TwistStamped -from tr_hat_msgs.msg import MotorPayload +from tr_hat_msgs.msg import MotorPower from driver import DriverStraight, DriverDifferential from controller import Controller @@ -47,7 +47,7 @@ def __init__(self): self.motor_pub = rospy.Publisher( "tr_hat_bridge/motors", - MotorPayload, + MotorPower, queue_size=1 ) @@ -72,5 +72,5 @@ def callback_cmd_stamped(self, data): self.callback_cmd(data.twist) - def send_payload(self): - self.motor_pub.publish(self.controller.payload) + def publish_motors(self): + self.motor_pub.publish(self.controller.power) diff --git a/tr_hat_bridge/README.md b/tr_hat_bridge/README.md index 6e35cee..e4e8519 100644 --- a/tr_hat_bridge/README.md +++ b/tr_hat_bridge/README.md @@ -8,10 +8,9 @@ Exposes ROS API for communication with Turtle Hat #### Subscribed Topics -* **`~motors`** ([tr_hat_msgs/MotorPayload]) +* **`~motors`** ([tr_hat_msgs/MotorPower]) - Sends the motor payload values described by 8-bit integers. - Values between 0-127 (0x00-0x7F) move the motors forward and values 128-255 (0x80-0xFF) move them backward + Sets motor power levels described by float values between -1.0 and 1.0 #### Services @@ -29,6 +28,6 @@ Exposes ROS API for communication with Turtle Hat Serial device connected to Turtle Hat -[tr_hat_msgs/MotorPayload]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/msg/MotorPayload.msg +[tr_hat_msgs/MotorPower]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/msg/MotorPower.msg [tr_hat_msgs/GetBattery]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/srv/GetBattery.srv [tr_hat_msgs/GetFirmwareVer]: https://github.com/TurtleRover/tr_ros/blob/master/tr_hat_msgs/srv/GetFirmwareVer.srv \ No newline at end of file diff --git a/tr_hat_bridge/src/tr_hat_bridge/bridge.py b/tr_hat_bridge/src/tr_hat_bridge/bridge.py index 388e8dd..be747fb 100644 --- a/tr_hat_bridge/src/tr_hat_bridge/bridge.py +++ b/tr_hat_bridge/src/tr_hat_bridge/bridge.py @@ -1,7 +1,7 @@ from __future__ import with_statement import rospy -from tr_hat_msgs.msg import MotorPayload, ServoAngle +from tr_hat_msgs.msg import MotorPower, ServoAngle from tr_hat_msgs.srv import (GetBattery, GetBatteryResponse, GetFirmwareVer, GetFirmwareVerResponse) @@ -24,7 +24,7 @@ def __init__(self): self.motor_sub = rospy.Subscriber( "~motors", - MotorPayload, + MotorPower, self.set_motors ) @@ -47,9 +47,17 @@ def __init__(self): ) def set_motors(self, data): + payload = [] + for p in data.power: + p = max(min(p, 1.0), -1.0) + value = int(round(p * 0x7F)) + if value < 0: + value = -value + 0x7F + payload.append(value) + with self.lock: self.comm.serial.flushInput() - f = frame.motors(data.payload) + f = frame.motors(payload) self.comm.send(f) status = self.comm.readline() diff --git a/tr_hat_msgs/CMakeLists.txt b/tr_hat_msgs/CMakeLists.txt index aff5253..f629270 100644 --- a/tr_hat_msgs/CMakeLists.txt +++ b/tr_hat_msgs/CMakeLists.txt @@ -8,7 +8,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( DIRECTORY msg FILES - MotorPayload.msg + MotorPower.msg ServoAngle.msg ) diff --git a/tr_hat_msgs/msg/MotorPayload.msg b/tr_hat_msgs/msg/MotorPayload.msg deleted file mode 100644 index dec6127..0000000 --- a/tr_hat_msgs/msg/MotorPayload.msg +++ /dev/null @@ -1,2 +0,0 @@ -# FL, RL, FR, RR -uint8[4] payload \ No newline at end of file diff --git a/tr_hat_msgs/msg/MotorPower.msg b/tr_hat_msgs/msg/MotorPower.msg new file mode 100644 index 0000000..3cca4e2 --- /dev/null +++ b/tr_hat_msgs/msg/MotorPower.msg @@ -0,0 +1,3 @@ +# RR, RL. FR. FL +# values between -1.0 and 1.0 +float32[4] power \ No newline at end of file