Error publishing to rosserial node
I am running a rosserial
node on an Arduino, I am using ROS Noetic. I initialize the node using the following:
<launch>
<node pkg="rosserial_python" type="serial_node.py" name="motor_control" output="screen">
<param name="~port" value="/dev/ttyACM0" />
<param name="~baud" value="57600" />
</node>
</launch>
The connection is established as shown below:
process[motor_control-1]: started with pid [5063]
[INFO] [1632597275.309702]: ROS Serial Python Node
[INFO] [1632597275.358828]: Connecting to /dev/ttyACM0 at 57600 baud
[INFO] [1632597277.494118]: Requesting topics...
[INFO] [1632597277.839086]: Note: subscribe buffer size is 280 bytes
[INFO] [1632597277.847758]: Setup subscriber on velocity_command [geometry_msgs/Twist]
I publish the msg from the command line:
rostopic pub /velocity_command geometry_msgs/Twist "{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0, z: 0.0}}"
but when I publish a command to the topic that the node subscribes to I get the following error:
Exception in thread Thread-6:
Traceback (most recent call last):
File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
self.run()
File "/usr/lib/python3.8/threading.py", line 870, in run
self._target(*self._args, **self._kwargs)
File "/home/ubuntu/catkin_ws/src/rosserial/rosserial_python/src/rosserial_python/SerialClient.py", line 778, in processWriteQueue
self._send(topic, msg)
File "/home/ubuntu/catkin_ws/src/rosserial/rosserial_python/src/rosserial_python/SerialClient.py", line 758, in _send
topic_bytes = struct.pack('<h', topic)
struct.error: short format requires (-0x7fff - 1) <= number <= 0x7fff
[INFO] [1632597808.464485]: Sending tx stop request
This error seems a bit cryptic and I am not quite sure how to tackle it. Any ideas what it may be due to?
Subscriber:
// Differential drive kinematics conversion to motor control inputs.
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>
#include <knet_bot_msgs/EncoderSpeed.h>
#include "CytronMotorDriver.h"
// Constant definitions
#define PI 3.1415926535897932384626433832795
// Robot parameters
float gear_ratio = 98.78;
float wheel_radius = 0.08;
float base_length = 0.171;
float power_supply_voltage = 24.0;
// Motor parameters
float max_motor_rpm = 10000;
float max_motor_voltage = 12.0;
float max_wheel_rpm = max_motor_rpm / gear_ratio;
float max_serial_scale_speed = 250.0 / (power_supply_voltage / max_motor_voltage);
float max_wheel_linear_speed = max_wheel_rpm * 2.0 * PI * wheel_radius / 60.0; // m/sec
// Feedback parameters
float right_motor_rpm;
float left_motor_rpm;
// Configure the motor driver.
CytronMD motor_right(PWM_DIR, 9, 8); // PWM 1 = Pin 3, DIR 1 = Pin 4.
CytronMD motor_left(PWM_DIR, 11, 13); // PWM 2 = Pin 9, DIR 2 = Pin
// Initialize ROS parameters
ros::NodeHandle nh;
void motor_callback(const geometry_msgs::Twist& velocity_msg){
nh.loginfo("Message to motor callback received.");
// Velocity inputs are v_x and w_z. Others are 0.
float diff_drive_right_speed = 1/wheel_radius * velocity_msg.linear.x + base_length/(2*wheel_radius) * velocity_msg.angular.z;
float diff_drive_left_speed = 1/wheel_radius * velocity_msg.linear.x - base_length/(2*wheel_radius) * velocity_msg.angular.z;
// Check speed saturation limits.
if(abs(diff_drive_right_speed) > max_wheel_linear_speed){
diff_drive_right_speed = diff_drive_right_speed/abs(diff_drive_right_speed) * max_wheel_linear_speed;
}
if(abs(diff_drive_left_speed) > max_wheel_linear_speed){
diff_drive_left_speed = diff_drive_left_speed/abs(diff_drive_left_speed) * max_wheel_linear_speed;
}
// Convert wheel linear speeds to motor angular velocity in RPM
float motor_right_speed_input_rpm = 60.0 * diff_drive_right_speed * gear_ratio / (2.0 * PI * wheel_radius);
float motor_left_speed_input_rpm = 60.0 * diff_drive_left_speed * gear_ratio / (2.0 * PI * wheel_radius ...
Can you include the subscriber callback code?
I have edited the post and added the subscriber code.
Looks like a cross-post of ros-drivers/rosserial#571.
I was not sure where this should be posted. I can remove one of them. Thank you.