最近在研究智能小车的搭建,ROS主控使用的是RK3399,电机驱动使用的是STM32,
RK3399和STM32使用USB线连接,并使用了rosserial_python进行数据传输,
我使用的是网上最经典最成熟的代码框架,话题订阅的结构如下:
some_other_code;
ros::NodeHandle nh;
some_other_code;
void command_callback( const geometry_msgs::Twist& cmd_msg);
some_other_code;
ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &command_callback );
some_other_code;
void command_callback( const geometry_msgs::Twist& cmd_msg)
{
char buffer[300];
required_linear_vel_x = cmd_msg.linear.x;
required_linear_vel_y = cmd_msg.linear.y;
required_angular_vel_z = cmd_msg.angular.z;
previous_command_time = millis();
sprintf (buffer, "NaclChan -----------------------------");
nh.loginfo(buffer);
sprintf (buffer, "NaclChan last cmd %d",previous_command_time);
nh.loginfo(buffer);
}
int main(void)
{
some_other_code;
SystemInit();
initialise();
some_other_code;
nh.initNode();
nh.advertise(raw_vel_pub);
some_other_code;
nh.subscribe(cmd_sub);
some_other_code;
while (!nh.connected())
{
nh.spinOnce();
}
sprintf (buffer, "NaclChan X5 Connected!");
nh.loginfo(buffer);
while(1)
{
if ((millis() - previous_control_time) >= (1000 / COMMAND_RATE))
{
move_base();
previous_control_time = millis();
}
some_other_code;
nh.spinOnce();
}
}
RK3399端运行了roslaunch,发现串口通信已经建立,STM32的话题发布和订阅也都有了信息的输出,如下:
[INFO] [1672405665.904525]: NaclChan ROS Serial Python Node
[INFO] [1672405665.938368]: NaclChan Connecting to /dev/rikibase at 115200 baud
[INFO] [1672405668.156120]: Note: publish buffer size is 1024 bytes
[INFO] [1672405668.158158]: Setup publisher on raw_vel [riki_msgs/Velocities]
[INFO] [1672405668.171965]: Setup publisher on raw_imu [riki_msgs/Imu]
**[INFO] [1672405668.186382]: Setup publisher on battery [riki_msgs/Battery]**
[INFO] [1672405668.203644]: Setup publisher on temperature_humidity [riki_msgs/DHT22]
[INFO] [1672405668.221608]: Setup publisher on sonar [riki_msgs/Sonar]
[INFO] [1672405668.256291]: Note: subscribe buffer size is 1024 bytes
[INFO] [1672405668.260395]: Setup subscriber on pid [riki_msgs/PID]
**[INFO] [1672405668.284116]: Setup subscriber on cmd_vel [geometry_msgs/Twist]**
[INFO] [1672405668.323482]: Setup subscriber on servo [riki_msgs/Servo]
[INFO] [1672405668.328804]: NaclChan X5 Connected!
其中 [riki_msgs/Battery]是STM32发布的话题,RK3399端使用rostopic echo /Battery能够看到发布的数据,证明STM32和RK3399间的物理连接是OK的,串口通信也是OK的。
但现在的问题是,STM32订阅了cmd_vel [geometry_msgs/Twist],咋的都接不到数据。我在RK3399端运行teleop_twist_keyboard,用键盘输入的方式往cmd_vel中写入数据,或者使用rostopic pub 指令往cmd_vel中写数据,都可以被rostopic echo cmd_vel监测到,但STM32始终没有任何反应,回调函数command_callback()根本就没有运行。
被这个问题卡了好久了,不知道咋继续调查了。希望指点