ROS2 cmd_vel subscriber & odometry publisher - subscription callback never called
I am writing a motor controller package in C++ that subscribes to /cmdvel and publishes to /odom. The node runs correctly and rqtgraph shows the subscription and publishers are connected. The problem is the callback function for /cmdvel subscriber never gets called at all. The run() function is called in the constructor, which results in odomloop() being called to calculate and build odometry messages. The cmdvelloop() function just has code used for testing in it. Is the while loop in the run() function causing the callback function to never be called? Does this require a multithreaded executor to handle my run() loop that publishes odometry and another thread to handle the /cmdvel subscription callback? Or would simply moving the run() function call into the sub callback and removing the while loop fix the issue?
I have the code that works with ROS1, and I used the general idea of that for this version. The code is 500 lines long so I will show the general structure:
namespace mc{
mc::mc : Node("driver")
// set parameters & initialize
variables
run()
}
void mc::cmdvel_callback(const
geometry_msgs::msg::Twist::SharedPtr
twist_msg)
{
// wheel speed (m/s)
float right_speed = twist_msg->linear.x + track_width *
twist_msg->angular.z / 2.0;
float left_speed = twist_msg->linear.x - track_width *
twist_msg->angular.z / 2.0;
// send motor commands to mc
}
void mc::cmdvel_loop()
{
//test code
}
int mc::run(){
// connect to serial port
while (rclcpp::ok())
{
cmdvel_loop();
odom_loop(); // reads encoder count & publishes odometry
}
}
return 0;
{
int main(int argc, char* argv[])
{
rclcpp::init(argc, argv);
rclcpp::executors::SingleThreadedExecutor exec;
rclcpp::NodeOptions options;
auto node = std::make_shared<mc::mc>();
exec.add_node(node);
exec.spin();
rclcpp::shutdown();
return 0;
}
Asked by chased11 on 2023-03-30 21:51:18 UTC
Answers
Correct me if I'm mistaken but in ROS2 /cmd_vel
does not provide a callback message. Meaning, you need to run spin_once()
instead of spin()
. This is because spin_once()
dosn't wait for a termination message unlike spin()
, which ends the cycle if and only if it's explicitly interrupted or a termination signal is received.
Asked by i1Cps on 2023-05-25 21:03:18 UTC
Comments
I'm not sure if that would have worked. Instead, I used exec.spin() in the main method as it's shown, removed the while loop in run(), and added
using namespace std::chrono_literals;
// set odometry publishing loop timer at 10Hz
timer_ = this->create_wall_timer(10ms,std::bind(&mc::run, this));
To the constructor, which calls the run() function every 10 ms and sends motor commands and publishes odometry. I wasn't aware of this method since this was one of my first ROS2 packages.
Asked by chased11 on 2023-05-26 13:43:51 UTC
Comments