Hi there.
I’m new to ROS and i’m facing a problem.
My class has:
1 subscriber: get scan data and update object parameters
1 service: make the robot move.
My question is:
it seems that when the service is called, the subscriber stops to read data from topic but the service calback needs realtime data from subscriber msg. Is thee a way to achieve this?
P.S. coding in C++.
Thank you all.
can you share the code?
Hi @LorenzoDagostino ,
Welcome to this Community!
Whenever you share a code, please use a fenced code-block. Also share the whole code so we can debug.
Refer: Extended Syntax | Markdown Guide
Regards,
Girish
#include <chrono>
#include <cstdio>
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/executors.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/subscription.hpp"
#include "custom_srvs/srv/find_wall.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
using LaserScan = sensor_msgs::msg::LaserScan;
using Twist = geometry_msgs::msg::Twist;
using FindWall = custom_srvs::srv::FindWall;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::literals::chrono_literals;
using namespace std::chrono;
class Robot : public rclcpp::Node {
public:
Robot(float min_r, float max_r, float max_f) : rclcpp::Node("robot") {
this->min_right_distance = min_r;
this->max_right_distance = max_r;
this->max_front_distance = max_f;
this->log("Creating Scanner.");
this->laser_subscriber_ = this->create_subscription<LaserScan>(
"scan", 10, std::bind(&Robot::laser_clbk, this, _1));
this->log("Creating Twist.");
this->twist_publisher_ = this->create_publisher<Twist>("cmd_vel", 10);
this->log("Creating service: find_wall.");
this->find_wall_srv_ = create_service<FindWall>(
"find_wall", std::bind(&Robot::go_to_wall, this, _1, _2));
this->log("Done.");
}
void set_x_linear(float x_);
void set_z_angular(float z_);
float *get_scan_readings(); // left, front, right readings
void move_forward();
void move_forward(float vel);
// void move_forward(uint8_t time);
void move_forward(float vel, uint8_t time);
void turn(bool left_right, uint8_t time);
void turn(bool left_right, float vel, uint8_t time);
void stop_robot();
void go_to_wall(const std::shared_ptr<FindWall::Request> request,
const std::shared_ptr<FindWall::Response> response);
const uint16_t right_idx = 0;
const uint16_t front_idx = 360;
const uint16_t left_idx = 719;
static const bool LEFT = 0;
static const bool RIGHT = 1;
private:
rclcpp::Service<FindWall>::SharedPtr find_wall_srv_;
float min_right_distance;
float max_right_distance;
float max_front_distance;
rclcpp::Subscription<LaserScan>::SharedPtr laser_subscriber_;
void laser_clbk(LaserScan::SharedPtr scan_reading);
float scanner_left;
float scanner_right;
float scanner_front;
rclcpp::Publisher<Twist>::SharedPtr twist_publisher_;
void publish_cmd_vel(Twist twist);
void publish_cmd_vel();
float x_linear;
float z_angular;
void log(std::string message);
};
void Robot::log(std::string message) {
RCLCPP_INFO(this->get_logger(), message);
}
void Robot::publish_cmd_vel(Twist twist) {
// This uses a custom value to publish message
char log_msg[100];
std::sprintf(log_msg, "Publishing: x = %f, z = %f", twist.linear.x,
twist.angular.z);
this->log(log_msg);
this->twist_publisher_->publish(twist);
this->log("Published");
}
void Robot::publish_cmd_vel() {
// This uses the stored values to publish message
Twist to_publish;
to_publish.linear.x = this->x_linear;
to_publish.angular.z = this->z_angular;
this->publish_cmd_vel(to_publish);
}
void Robot::laser_clbk(LaserScan::SharedPtr scan_reading) {
this->scanner_left = (float)(scan_reading->ranges[Robot::left_idx]);
this->scanner_right = (float)(scan_reading->ranges[Robot::right_idx]);
this->scanner_front = (float)(scan_reading->ranges[Robot::front_idx]);
}
void Robot::set_x_linear(float x_) { this->x_linear = x_; }
void Robot::set_z_angular(float z_) { this->z_angular = z_; }
float *Robot::get_scan_readings() {
static float ret[] = {this->scanner_left, this->scanner_front,
this->scanner_right};
return ret;
}
void Robot::move_forward(float vel) {
Twist twist;
twist.linear.x = vel;
twist.angular.z = 0.0;
this->publish_cmd_vel(twist);
}
void Robot::move_forward() {
Twist twist;
twist.linear.x = this->x_linear;
twist.angular.z = 0.0;
this->publish_cmd_vel(twist);
}
void Robot::move_forward(float vel, uint8_t time) {
this->move_forward(vel);
auto beg = high_resolution_clock::now();
while (duration_cast<seconds>(high_resolution_clock::now() - beg).count() <
time) {
std::this_thread::sleep_for(100ms);
}
this->stop_robot();
}
void Robot::stop_robot() {
Twist t;
t.linear.x = 0;
t.angular.z = 0;
this->publish_cmd_vel(t);
}
void Robot::turn(bool left_right, float vel, uint8_t time) {
if (vel == 0.0) {
this->log("Angular velocity must be != 0.");
return;
}
int8_t factor = (left_right) ? -1 : 1;
Twist t;
t.linear.x = 0;
t.angular.z = factor * fabs(vel);
this->publish_cmd_vel(t);
auto beg = high_resolution_clock::now();
while (duration_cast<seconds>(high_resolution_clock::now() - beg).count() <
time) {
std::this_thread::sleep_for(100ms);
}
this->stop_robot();
}
void Robot::turn(bool left_right, uint8_t time) {
if (this->z_angular == 0.0) {
this->log("Internal angular velocity = 0.");
}
this->turn(left_right, this->z_angular, time);
}
void Robot::go_to_wall(const std::shared_ptr<FindWall::Request> request,
const std::shared_ptr<FindWall::Response> response) {
this->move_forward(0.1);
float *scan = this->get_scan_readings();
while (scan[1] > this->max_front_distance) {
// std::this_thread::sleep_for(100ms);
scan = this->get_scan_readings();
}
this->stop_robot();
this->turn(Robot::LEFT, 1.4, 1);
response->wallfound = true;
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Robot>(0.3, 0.5, 0.5));
rclcpp::shutdown();
return 0;
}
Hi @LorenzoDagostino ,
After going through your code I notice the following:
/scan?laser_callback and service_callback as two parallel processes by declaring them as ReentrantCallbackGroup.MultiThreadedExecutor to avoid this “blocking” that happens in your code - when the service callback executes, your laser callback hangs - because service callback blocks your laser callback.If you properly implement the above 3 steps, your problem should be solved!
Regards,
Girish
@girishkumar.kannan thank you so mutch. It worked. Working code for completeness
#include <chrono>
#include <cstdio>
#include <functional>
#include <memory>
#include <thread>
#include "rclcpp/executors.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/subscription.hpp"
#include "custom_srvs/srv/find_wall.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
using LaserScan = sensor_msgs::msg::LaserScan;
using Twist = geometry_msgs::msg::Twist;
using FindWall = custom_srvs::srv::FindWall;
using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::literals::chrono_literals;
using namespace std::chrono;
class Robot : public rclcpp::Node {
public:
Robot(float min_r, float max_r, float max_f) : rclcpp::Node("robot") {
this->min_right_distance = min_r;
this->max_right_distance = max_r;
this->max_front_distance = max_f;
auto default_qos = rclcpp::QoS(rclcpp::SystemDefaultsQoS());
this->log("Creating Scanner.");
this->callback_group_ =
this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
rclcpp::SubscriptionOptions options;
options.callback_group = this->callback_group_ ;
this->laser_subscriber_ = this->create_subscription<LaserScan>(
"scan", default_qos, std::bind(&Robot::laser_clbk, this, _1), options);
this->log("Creating Twist.");
this->twist_publisher_ = this->create_publisher<Twist>("cmd_vel", 10);
this->log("Creating service: find_wall.");
this->find_wall_srv_ = create_service<FindWall>(
"find_wall", std::bind(&Robot::go_to_wall, this, _1, _2),
rmw_qos_profile_services_default, callback_group_);
this->log("Done.");
}
void set_x_linear(float x_);
void set_z_angular(float z_);
float *get_scan_readings(); // left, front, right readings
void move_forward();
void move_forward(float vel);
// void move_forward(uint8_t time);
void move_forward(float vel, uint8_t time);
void turn(bool left_right, uint8_t time);
void turn(bool left_right, float vel, uint8_t time);
void stop_robot();
void go_to_wall(const std::shared_ptr<FindWall::Request> request,
const std::shared_ptr<FindWall::Response> response);
const uint16_t right_idx = 0;
const uint16_t front_idx = 360;
const uint16_t left_idx = 719;
static const bool LEFT = 0;
static const bool RIGHT = 1;
private:
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::Service<FindWall>::SharedPtr find_wall_srv_;
float min_right_distance;
float max_right_distance;
float max_front_distance;
rclcpp::Subscription<LaserScan>::SharedPtr laser_subscriber_;
void laser_clbk(LaserScan::SharedPtr scan_reading);
float scanner_left;
float scanner_right;
float scanner_front;
rclcpp::Publisher<Twist>::SharedPtr twist_publisher_;
void publish_cmd_vel(Twist twist);
void publish_cmd_vel();
float x_linear;
float z_angular;
void log(std::string message);
};
void Robot::log(std::string message) {
RCLCPP_INFO(this->get_logger(), message);
}
void Robot::publish_cmd_vel(Twist twist) {
// This uses a custom value to publish message
char log_msg[100];
std::sprintf(log_msg, "Publishing: x = %f, z = %f", twist.linear.x,
twist.angular.z);
this->log(log_msg);
this->twist_publisher_->publish(twist);
this->log("Published");
}
void Robot::publish_cmd_vel() {
// This uses the stored values to publish message
Twist to_publish;
to_publish.linear.x = this->x_linear;
to_publish.angular.z = this->z_angular;
this->publish_cmd_vel(to_publish);
}
void Robot::laser_clbk(LaserScan::SharedPtr scan_reading) {
this->scanner_left = (float)(scan_reading->ranges[Robot::left_idx]);
this->scanner_right = (float)(scan_reading->ranges[Robot::right_idx]);
this->scanner_front = (float)(scan_reading->ranges[Robot::front_idx]);
}
void Robot::set_x_linear(float x_) { this->x_linear = x_; }
void Robot::set_z_angular(float z_) { this->z_angular = z_; }
float *Robot::get_scan_readings() {
static float ret[] = {this->scanner_left, this->scanner_front,
this->scanner_right};
return ret;
}
void Robot::move_forward(float vel) {
Twist twist;
twist.linear.x = vel;
twist.angular.z = 0.0;
this->publish_cmd_vel(twist);
}
void Robot::move_forward() {
Twist twist;
twist.linear.x = this->x_linear;
twist.angular.z = 0.0;
this->publish_cmd_vel(twist);
}
void Robot::move_forward(float vel, uint8_t time) {
this->move_forward(vel);
auto beg = high_resolution_clock::now();
while (duration_cast<seconds>(high_resolution_clock::now() - beg).count() <
time) {
std::this_thread::sleep_for(100ms);
}
this->stop_robot();
}
void Robot::stop_robot() {
Twist t;
t.linear.x = 0;
t.angular.z = 0;
this->publish_cmd_vel(t);
}
void Robot::turn(bool left_right, float vel, uint8_t time) {
if (vel == 0.0) {
this->log("Angular velocity must be != 0.");
return;
}
int8_t factor = (left_right) ? -1 : 1;
Twist t;
t.linear.x = 0;
t.angular.z = factor * fabs(vel);
this->publish_cmd_vel(t);
auto beg = high_resolution_clock::now();
while (duration_cast<seconds>(high_resolution_clock::now() - beg).count() <
time) {
std::this_thread::sleep_for(100ms);
}
this->stop_robot();
}
void Robot::turn(bool left_right, uint8_t time) {
if (this->z_angular == 0.0) {
this->log("Internal angular velocity = 0.");
}
this->turn(left_right, this->z_angular, time);
}
void Robot::go_to_wall(const std::shared_ptr<FindWall::Request> request,
const std::shared_ptr<FindWall::Response> response) {
this->move_forward(0.1);
while (this->scanner_front > 0.25) {
std::this_thread::sleep_for(50ms);
}
this->stop_robot();
this->turn(Robot::LEFT, 1.38, 2);
response->wallfound = true;
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
auto robot = std::make_shared<Robot>(0.3, 0.5, 0.5);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(robot);
executor.spin();
rclcpp::shutdown();
return 0;
}
Hi @LorenzoDagostino ,
Glad to know you got it fixed and working!
Now that your problem is fixed, please edit all your posts and remove your code. You are making your solution public, by posting it publicly. Although there is no problem if you have it posted for a short while (less than 2 days).
Regards,
Girish
This topic was automatically closed 10 days after the last reply. New replies are no longer allowed.