diff --git a/README.md b/README.md index b0d9c13..376b0ff 100644 --- a/README.md +++ b/README.md @@ -1,65 +1,65 @@ -# ROS package for ZLAC8015D dual-channel servo driver -[![](https://img.shields.io/badge/ROS-Noetic-brightgreen.svg)](https://github.com/Alpaca-zip/zlac8015d_ros) - -![circuit_scheme](https://user-images.githubusercontent.com/84959376/191182537-47d49329-7e61-4c6c-b6a7-7eab8f530f07.png) - -## 1 Installation -``` -$ cd catkin_ws/src - -$ git clone https://github.com/Alpaca-zip/zlac8015d_ros.git - -$ cd .. && catkin_make -``` -## 2 Usage -### Run motor_driver_node -``` -$ roslaunch zlac8015d_ros motor_driver_node.launch -``` -### Parameters -- `port`: Name of the zlac8015d port. Default is `dev/ttyUSB0`. -- `control_mode`: `1` is relative position control mode, `3` is speed rpm control mode. Default is `3`. -- `debug`: If `true`, odometry information is displayed. Default is `false`. -- `twist_cmd_vel_topic`: Topic name for Twist-type messages. Default is `/zlac8015d/twist/cmd_vel`. -- `cmd_vel_topic`: Topic name for cmd_vel[m/s] messages. Default is `/zlac8015d/vel/cmd_vel`. -- `cmd_rpm_topic`: Topic name for cmd_rpm messages. Default is `/zlac8015d/vel/cmd_rpm`. -- `cmd_deg_topic`: Topic name for cmd_deg[°] messages. Default is `/zlac8015d/pos/cmd_deg`. -- `cmd_dist_topic`: Topic name for cmd_dist[m] messages. Default is `/zlac8015d/pos/cmd_dist`. -- `publish_TF`: If `true`, TF is published. Default is `true`. -- `TF_header_frame`: Name of the TF header frame. Default is `odom`. -- `TF_child_frame`: Name of the TF child frame. Default is `base_link`. -- `publish_odom`: If `true`, `/odom` is published. Default is `true`. -- `odom_header_frame`: Name of the odometry header frame. Default is `odom`. -- `odom_child_frame`: Name of the odometry child frame. Default is `base_link`. -- `left_wheel_radius`: Left wheel radius. Default is `0.1015`[m]. -- `right_wheel_radius`: Right wheel radius. Default is `0.1015`[m]. -- `computation_left_wheel_radius`: Radius of left wheel used for odometry computation. Default is `0.1015`[m]. -- `computation_right_wheel_radius`: Radius of right wheel used for odometry computation. Default is `0.1015`[m]. -- `cpr`: CPR(Counts Per Revolution). Default is `16385`. -- `wheels_base_width`: Distance between tires. Default is `0.5668`[m]. -- `callback_timeout`: Motor automatically stops if no topics are received for a certain period of time. Default is `0.5`[s]. -- `set_accel_time_left`: Acceleration time for left tire. Default is `200`[ms]. -- `set_accel_time_right`: Acceleration time for right tire. Default is `200`[ms]. -- `set_decel_time_left`: Deceleration time for left tire. Default is `200`[ms]. -- `set_decel_time_right`: Deceleration time for right tire. Default is `200`[ms]. -- `max_left_rpm`: Maximum rpm of left tire. Default is `150`. -- `max_right_rpm`: Maximum rpm of right tire. Default is `150`. -- `deadband_rpm`: Width of rpm to be regarded as 0. If `3`, then -3 to 3 is considered rpm 0. Default is `3`. - -### Topics -This node publishes the following topics. -- `/wheels_rpm`: The speed in RPM of each tire as [left, right]. -- `/odom`: Odometry data for the robot. [More detail](http://docs.ros.org/en/diamondback/api/nav_msgs/html/msg/Odometry.html) - -This node subscribes to the following topics. -- `/zlac8015d/twist/cmd_vel`: Send command as linear velocity and angular velocity in speed rpm control. [More detail](https://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/Twist.html) -- `/zlac8015d/vel/cmd_vel`: Send command as velocity in speed rpm control, e.g. [0.6, 0.5] 0.6[m/s] of left tire, 0.5[m/s] of right tire. -- `/zlac8015d/vel/cmd_rpm`: Send command as rpm in speed rpm control, e.g. [100, 50] 100 rpm of left wtire, 50 rpm of right tire. -- `/zlac8015d/pos/deg_cmd`: Send command as angle degree in position control, e.g. [90,70] 90 [deg] of left tire, 70 [deg] of right tire. -- `/zlac8015d/pos/dist_cmd`: Send command as desired travelling distance in position control, e.g. [1.0, 1.0] for 1[m] travelling distance of each tire. -- `/estop`: Send command as emergency stop signal, e.g. true for emergency stop is activated. - -### Emergency stop Feature -The motor is locked when you publish a `true` message on the `/estop` topic. - -This feature is not recommended. Emergency stop should be controlled by hardware. +# ROS package for ZLAC8015D dual-channel servo driver +[![](https://img.shields.io/badge/ROS-Noetic-brightgreen.svg)](https://github.com/Alpaca-zip/zlac8015d_ros) + +![circuit_scheme](https://user-images.githubusercontent.com/84959376/191182537-47d49329-7e61-4c6c-b6a7-7eab8f530f07.png) + +## 1 Installation +``` +$ cd catkin_ws/src + +$ git clone https://github.com/Alpaca-zip/zlac8015d_ros.git + +$ cd .. && catkin_make +``` +## 2 Usage +### Run motor_driver_node +``` +$ roslaunch zlac8015d_ros motor_driver_node.launch +``` +### Parameters +- `port`: Name of the zlac8015d port. Default is `dev/ttyUSB0`. +- `control_mode`: `1` is relative position control mode, `3` is speed rpm control mode. Default is `3`. +- `debug`: If `true`, odometry information is displayed. Default is `false`. +- `twist_cmd_vel_topic`: Topic name for Twist-type messages. Default is `/zlac8015d/twist/cmd_vel`. +- `cmd_vel_topic`: Topic name for cmd_vel[m/s] messages. Default is `/zlac8015d/vel/cmd_vel`. +- `cmd_rpm_topic`: Topic name for cmd_rpm messages. Default is `/zlac8015d/vel/cmd_rpm`. +- `cmd_deg_topic`: Topic name for cmd_deg[°] messages. Default is `/zlac8015d/pos/cmd_deg`. +- `cmd_dist_topic`: Topic name for cmd_dist[m] messages. Default is `/zlac8015d/pos/cmd_dist`. +- `publish_TF`: If `true`, TF is published. Default is `true`. +- `TF_header_frame`: Name of the TF header frame. Default is `odom`. +- `TF_child_frame`: Name of the TF child frame. Default is `base_link`. +- `publish_odom`: If `true`, `/odom` is published. Default is `true`. +- `odom_header_frame`: Name of the odometry header frame. Default is `odom`. +- `odom_child_frame`: Name of the odometry child frame. Default is `base_link`. +- `left_wheel_radius`: Left wheel radius. Default is `0.1015`[m]. +- `right_wheel_radius`: Right wheel radius. Default is `0.1015`[m]. +- `computation_left_wheel_radius`: Radius of left wheel used for odometry computation. Default is `0.1015`[m]. +- `computation_right_wheel_radius`: Radius of right wheel used for odometry computation. Default is `0.1015`[m]. +- `cpr`: CPR(Counts Per Revolution). Default is `16385`. +- `wheels_base_width`: Distance between tires. Default is `0.5668`[m]. +- `callback_timeout`: Motor automatically stops if no topics are received for a certain period of time. Default is `0.5`[s]. +- `set_accel_time_left`: Acceleration time for left tire. Default is `200`[ms]. +- `set_accel_time_right`: Acceleration time for right tire. Default is `200`[ms]. +- `set_decel_time_left`: Deceleration time for left tire. Default is `200`[ms]. +- `set_decel_time_right`: Deceleration time for right tire. Default is `200`[ms]. +- `max_left_rpm`: Maximum rpm of left tire. Default is `150`. +- `max_right_rpm`: Maximum rpm of right tire. Default is `150`. +- `deadband_rpm`: Width of rpm to be regarded as 0. If `3`, then -3 to 3 is considered rpm 0. Default is `3`. + +### Topics +This node publishes the following topics. +- `/wheels_rpm`: The speed in RPM of each tire as [left, right]. +- `/odom`: Odometry data for the robot. [More detail](http://docs.ros.org/en/diamondback/api/nav_msgs/html/msg/Odometry.html) + +This node subscribes to the following topics. +- `/zlac8015d/twist/cmd_vel`: Send command as linear velocity and angular velocity in speed rpm control. [More detail](https://docs.ros.org/en/diamondback/api/geometry_msgs/html/msg/Twist.html) +- `/zlac8015d/vel/cmd_vel`: Send command as velocity in speed rpm control, e.g. [0.6, 0.5] 0.6[m/s] of left tire, 0.5[m/s] of right tire. +- `/zlac8015d/vel/cmd_rpm`: Send command as rpm in speed rpm control, e.g. [100, 50] 100 rpm of left wtire, 50 rpm of right tire. +- `/zlac8015d/pos/deg_cmd`: Send command as angle degree in position control, e.g. [90,70] 90 [deg] of left tire, 70 [deg] of right tire. +- `/zlac8015d/pos/dist_cmd`: Send command as desired travelling distance in position control, e.g. [1.0, 1.0] for 1[m] travelling distance of each tire. +- `/estop`: Send command as emergency stop signal, e.g. true for emergency stop is activated. + +### Emergency stop Feature +The motor is locked when you publish a `true` message on the `/estop` topic. + +This feature is not recommended. Emergency stop should be controlled by hardware. diff --git a/circuit_scheme.png b/circuit_scheme.png deleted file mode 100644 index 04897a1..0000000 --- a/circuit_scheme.png +++ /dev/null Binary files differ diff --git a/scripts/motor_driver_node.py b/scripts/motor_driver_node.py index b2551e1..7636ab9 100644 --- a/scripts/motor_driver_node.py +++ b/scripts/motor_driver_node.py @@ -50,6 +50,7 @@ self.L_FB_POS_HI = 0x20A7 # -----Control CMDs (REG)----- + self.EMER_STOP = 0x05 self.ALRM_CLR = 0x06 self.DOWN_TIME = 0x07 self.ENABLE = 0x08 @@ -102,12 +103,27 @@ self.estop = False self.last_subscribed_time = 0.0 + self.callback_timeout = float(rospy.get_param("/motor_driver_node/callback_timeout")) self.wheels_base_width = float(rospy.get_param("/motor_driver_node/wheels_base_width")) self.left_wheel_radius = float(rospy.get_param("/motor_driver_node/left_wheel_radius")) self.right_wheel_radius = float(rospy.get_param("/motor_driver_node/right_wheel_radius")) self.computation_left_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_left_wheel_radius")) self.computation_right_wheel_radius = float(rospy.get_param("/motor_driver_node/computation_right_wheel_radius")) + self.cpr = int(rospy.get_param("/motor_driver_node/cpr")) + self.deadband_rpm = int(rospy.get_param("/motor_driver_node/deadband_rpm")) + self.left_rpm_lim = int(rospy.get_param("/motor_driver_node/max_left_rpm")) + self.right_rpm_lim = int(rospy.get_param("/motor_driver_node/max_right_rpm")) + self.publish_TF = rospy.get_param("/motor_driver_node/publish_TF") + self.publish_odom = rospy.get_param("/motor_driver_node/publish_odom") + self.TF_header_frame = rospy.get_param("/motor_driver_node/TF_header_frame") + self.TF_child_frame = rospy.get_param("/motor_driver_node/TF_child_frame") + self.odom_header_frame = rospy.get_param("/motor_driver_node/odom_header_frame") + self.odom_child_frame = rospy.get_param("/motor_driver_node/odom_child_frame") + self.debug = rospy.get_param("/motor_driver_node/debug") self.period = 0.05 + self.x = 0.0 + self.y = 0.0 + self.theta = 0.0 self.l_meter = 0.0 self.r_meter = 0.0 self.prev_l_meter = 0.0 @@ -356,7 +372,7 @@ Convert from degree to 32bitArray. """ def deg_to_32bitArray(self, deg): - dec = int(self.map(deg, -1440, 1440, -65536, 65536)) + dec = int((deg + 1440) * (65536 + 65536) / (1440 + 1440) - 65536) HI_WORD = (dec & 0xFFFF0000) >> 16 LO_WORD = dec & 0x0000FFFF return [HI_WORD, LO_WORD] @@ -368,7 +384,6 @@ Get distance traveled by left and right wheels. """ def get_wheels_travelled(self): - cpr = int(rospy.get_param("/motor_driver_node/cpr")) registers = self.modbus_fail_read_handler(self.L_FB_POS_HI, 4) l_pul_hi = registers[0] l_pul_lo = registers[1] @@ -376,8 +391,8 @@ r_pul_lo = registers[3] l_pulse = np.int32(((l_pul_hi & 0xFFFF) << 16) | (l_pul_lo & 0xFFFF)) r_pulse = np.int32(((r_pul_hi & 0xFFFF) << 16) | (r_pul_lo & 0xFFFF)) - l_travelled = (float(l_pulse) / cpr) * self.computation_left_wheel_radius * np.pi * 8 # unit in meter - r_travelled = (float(r_pulse) / cpr) * self.computation_right_wheel_radius * np.pi * 8 # unit in meter + l_travelled = (float(l_pulse) / self.cpr) * self.computation_left_wheel_radius * np.pi * 8 # unit in meter + r_travelled = (float(r_pulse) / self.cpr) * self.computation_right_wheel_radius * np.pi * 8 # unit in meter return l_travelled, r_travelled """ @@ -407,29 +422,26 @@ Set RPM with limit. """ def set_rpm_with_limit(self, left_rpm, right_rpm): - deadband_rpm = int(rospy.get_param("/motor_driver_node/deadband_rpm")) - left_rpm_lim = int(rospy.get_param("/motor_driver_node/max_left_rpm")) - right_rpm_lim = int(rospy.get_param("/motor_driver_node/max_right_rpm")) - if left_rpm_lim < left_rpm: - left_rpm = left_rpm_lim + if self.left_rpm_lim < left_rpm: + left_rpm = self.left_rpm_lim rospy.logwarn("RPM reach the limit.") - elif left_rpm < -left_rpm_lim: - left_rpm = -left_rpm_lim + elif left_rpm < -self.left_rpm_lim: + left_rpm = -self.left_rpm_lim rospy.logwarn("RPM reach the limit.") - elif -deadband_rpm < left_rpm < deadband_rpm: + elif -self.deadband_rpm < left_rpm < self.deadband_rpm: left_rpm = 0 - if right_rpm_lim < right_rpm: - right_rpm = right_rpm_lim + if self.right_rpm_lim < right_rpm: + right_rpm = self.right_rpm_lim rospy.logwarn("RPM reach the limit.") - elif right_rpm < -right_rpm_lim: - right_rpm = -right_rpm_lim + elif right_rpm < -self.right_rpm_lim: + right_rpm = -self.right_rpm_lim rospy.logwarn("RPM reach the limit.") - elif -deadband_rpm < right_rpm < deadband_rpm: + elif -self.deadband_rpm < right_rpm < self.deadband_rpm: right_rpm = 0 - left_bytes = self.int16Dec_to_int16Hex(left_rpm) - right_bytes = self.int16Dec_to_int16Hex(right_rpm) + left_bytes = self.int16Dec_to_int16Hex(int(left_rpm)) + right_bytes = self.int16Dec_to_int16Hex(int(right_rpm)) result = self.client.write_registers(self.L_CMD_RPM, [left_bytes, right_bytes], unit = self.ID) """ @@ -471,10 +483,10 @@ theta = out_vector[2, 0] # -----Construct TF----- - if(rospy.get_param("/motor_driver_node/publish_TF")): + if(self.publish_TF): self.t.header.stamp = rospy.Time.now() - self.t.header.frame_id = rospy.get_param("/motor_driver_node/TF_header_frame") - self.t.child_frame_id = rospy.get_param("/motor_driver_node/TF_child_frame") + self.t.header.frame_id = self.TF_header_frame + self.t.child_frame_id = self.TF_child_frame self.t.transform.translation.x = x self.t.transform.translation.y = y self.t.transform.translation.z = 0.0 @@ -486,10 +498,10 @@ self.br.sendTransform(self.t) # -----Construct Odom Message----- - if(rospy.get_param("/motor_driver_node/publish_odom")): + if(self.publish_odom): self.odom_msg.header.stamp = rospy.Time.now() - self.odom_msg.header.frame_id = rospy.get_param("/motor_driver_node/odom_header_frame") - self.odom_msg.child_frame_id = rospy.get_param("/motor_driver_node/odom_child_frame") + self.odom_msg.header.frame_id = self.odom_header_frame + self.odom_msg.child_frame_id = self.odom_child_frame self.odom_msg.pose.pose.position.x = x self.odom_msg.pose.pose.position.y = y self.odom_msg.pose.pose.position.z = 0.0 @@ -591,14 +603,14 @@ self.got_pos_dist_cmd = False #-----Odometry computation----- - self.calculate_odometry() + x, y, theta = self.calculate_odometry() self.period = time.perf_counter() - start_time self.prev_l_meter = self.l_meter self.prev_r_meter = self.r_meter #-----Debugging Feature----- - if (rospy.get_param("/motor_driver_node/debug")): - print("\033[32m" + "x: {:f} | y: {:f} | yaw: {:f}".format(self.x, self.y, np.rad2deg(self.theta)) + "\033[0m") + if (self.debug): + print("\033[32m" + "x: {:f} | y: {:f} | yaw: {:f}".format(x, y, np.rad2deg(theta)) + "\033[0m") else: pass