I had some time on the real robot today and was trying to test my code. I have all of the parts of the ros2 project already implemented and tested in Sim. All of the behaviors work as they should in sim, but not so much when I tried it on the real robot.
Specifically, after the service client is used for finding and aligning the right side of the robot with the nearest wall. At this point, when the robot is positioned properly to follow the wall, the action server should receive the action goal and start recording odometry. Also at the same time, a subscriber to the laser scanner starts to move the robot along the wall using a publisher in it’s callback based on the laser values.
This is where I start running into issues. The movement of the robot when it is supposed to be following the wall and recording odometry is not continuous. It will execute a movement for a second, and then stay still for another(maybe more), and then execute the next /cmd_vel message. This ends up being really slow and choppy. Additionally, it seems that it only moves when the feedback from the action server is not being output. I suspected that the subscriber and action callbacks were blocking one another, so I created a callback group for each. I set the callback group type as “Reentrant” and passed them into the initialization statements. However, this still didn’t help with the movement of the robot. What am I missing here? I would greatly appreciate any help to get this done by the end of the week. Thank you.
Hello @reallen ,
As long as the callbacks are working OK in the simulation, there’s no reason why they should not work in the same way in the real robot. What might change, though, is the behavior of the cmd_vel topic. In simulation is common to work with latched topics. This means that, when sent a velocity command, the robot will keep moving until it receives a stop command (velocities to 0). In the real robot, though, the robot will automatically stop as soon as it stops receiving messages in the cmd_vel topic (for security reasons). This makes me think that the issue might be that you are not sending velocity commands at a high enough rate. Could you check this?
I can paste a portion of the code responsible for building the /cmd_vel messages and publishing them. This mentioned code is in the subscriber callback, which means that it should be publishing everytime the susbcriber receives any data from the /scan topic right? I did ros2 topic echo /cmd_vel and ros2 topic hz /cmd_vel, the topic echo showed that the message I was trying to send was being published to the topic and the topic hz showed a rate of around 6 hz.
void topic_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
// Action goal hasn't been sent, don't perform any movements
if (!action_goal_sent_) {
return;
}
rclcpp::Rate sleep_rate(1);
double infinity_ = std::numeric_limits<double>::infinity();
// // UNCOMMENT FOR SIM ROBOT
// Hard left
if (msg->ranges[180] < .4) {
RCLCPP_INFO(this->get_logger(), "Approaching wall, hard left");
// sim values
twist.linear.x = 0.05;
twist.angular.z = 0.75;
}
// turn right
else if (msg->ranges[90] > 0.27) {
RCLCPP_INFO(this->get_logger(), "turning right...");
// sim values
twist.linear.x = 0.1;
twist.angular.z = -0.135;
}
// turn left
else if (msg->ranges[90] < 0.24 or msg->ranges[90] == infinity_ or
msg->ranges[85] < 0.24 or msg->ranges[95] < 0.24) {
RCLCPP_INFO(this->get_logger(), "turning left...");
// sim values
twist.linear.x = 0.1;
twist.angular.z = 0.135;
}
// turn slightly left
else if (msg->ranges[135] < 0.24 or msg->ranges[135] == infinity_ or
msg->ranges[100] < 0.24 or msg->ranges[110] < 0.24 or
msg->ranges[120] < 0.24) {
RCLCPP_INFO(this->get_logger(), "turning slightly left...");
// sim values
twist.linear.x = 0.1;
twist.angular.z = 0.1;
}
// turn slighly right
else if (msg->ranges[45] > 0.27) {
RCLCPP_INFO(this->get_logger(), "turning slightly right...");
// sim values
twist.linear.x = 0.1;
twist.angular.z = -0.1;
}
// move forward
else if (msg->ranges[90] > 0.24 && msg->ranges[90] < 0.27) {
RCLCPP_INFO(this->get_logger(), "moving forward...");
// sim values
twist.linear.x = 0.15;
twist.angular.z = 0.0;
}
// END SIM robot
// send the movement message once logic is done
publisher_->publish(twist);
sleep_rate.sleep();
}
The trick that could work here would be to publish to the robot outside the callback, since the callback does not seem to run fast enough.
The twist message could be available within and outside the callback so that you publish it outside the callback while letting the callback update it. In such a case, the lag in the callback would affect the robot’s response when changing direction or speed, but the robot would be in motion all the time, limited perhaps by it’s factory speed limit.
That makes sense, I’ll give it a go. Just a note, rate of the messages being published to the /cmd_vel is at an average of 1 second. Is this information revealing of anything else besides the assumption that the twist message isn’t being published fast enough?