added use_safety_system parameter

Signed-off-by: Jakub Delicat <delicat.kuba@gmail.com>

compose.ros2aria.yaml

@@ -1,7 +1,7 @@
 # docker compose -f compose.ros2aria.yaml up
 services:
   ros2aria-dev:
-   image: delicjusz/pioneer
+   image: delicjusz/pioneer:powerbot
    network_mode: host
    ipc: host
    environment:
@@ -12,4 +12,4 @@ services:
     - ./fastdds.xml:/fastdds.xml
    devices:
     - /dev/ttyS0
-   command:  ros2 launch pioneer_bringup robot.launch.py namespace:=pioneer${PIONEER_ID}/
+   command:  ros2 run ros2aria ros2aria --ros-args -p num_of_sonars:=28

src/ros2aria/include/ros2aria/ros2aria.hpp

@@ -41,7 +41,7 @@ public:
     ~Ros2Aria();
 
 private:
-    std::size_t id;
+    std::size_t id{0};
     RAIIBot::SharedPtr robot;
     ArFunctorC<Ros2Aria> sensorCb;
     ros2aria_msgs::msg::RestrictionsMsg restrictions;
@@ -50,7 +50,9 @@ private:
     bool master_stop = true;
     bool user_stop = true;
     bool use_sonar = true;
+    bool use_safety_system = true;
     uint8_t num_of_sonars = 0;
+    std::string ros_namespace="";
 
     std::size_t extract_number_from_string(const std::string& str);
 

src/ros2aria/src/cmd_vel.cpp

@@ -14,16 +14,18 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
 
     auto r = robot->getRobot();
 
-    if (master_stop or obstacle_too_close or user_stop){
+    if(use_safety_system){
-        x = 0.0;
+        if (master_stop or obstacle_too_close or user_stop){
-        y = 0.0;
+            x = 0.0;
-        z = 0.0;
+            y = 0.0;
-    }
+            z = 0.0;
-    else{
+        }
-        // apply limits
+        else{
-        x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x;
+            // apply limits
-        y = std::abs(y) > restrictions.linear_velocity.data ? std::abs(y)/y * restrictions.linear_velocity.data : y;
+            x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x;
-        z = std::abs(z) > restrictions.angular_velocity.data ?  std::abs(z)/z * restrictions.angular_velocity.data : z;
+            y = std::abs(y) > restrictions.linear_velocity.data ? std::abs(y)/y * restrictions.linear_velocity.data : y;
+            z = std::abs(z) > restrictions.angular_velocity.data ?  std::abs(z)/z * restrictions.angular_velocity.data : z;
+        }
     }
     x *= 1e3;
     y *= 1e3;

src/ros2aria/src/pose.cpp

@@ -19,8 +19,8 @@ std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> Ros2Ari
     odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
     odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
 
-    odom_msg.header.frame_id = std::string(get_namespace()) + "/odom";
+    odom_msg.header.frame_id = ros_namespace + "/odom";
-    odom_msg.child_frame_id = std::string(get_namespace()) + "/base_link";
+    odom_msg.child_frame_id = ros_namespace + "/base_link";
     odom_msg.header.stamp = stamp;
 
     geometry_msgs::msg::TransformStamped transform;

src/ros2aria/src/ros2aria.cpp

@@ -9,27 +9,31 @@ Ros2Aria::Ros2Aria()
     : Node("ros2aria"),
       sensorCb(this, &Ros2Aria::publish)
 {
-    id = extract_number_from_string(std::string(get_namespace()));
+    if(get_namespace() != std::string("/")){
+        id = extract_number_from_string(std::string(get_namespace()));
+    }
 
     this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
     handle_parameters();
 
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
 
-    restrictions_sub_ = this->create_subscription<ros2aria_msgs::msg::RestrictionsMsg>(
+    if(use_safety_system){
-        "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1));
+        restrictions_sub_ = this->create_subscription<ros2aria_msgs::msg::RestrictionsMsg>(
-    init_restrictions();
+            "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1));
-
+        init_restrictions();
-    master_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        master_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
-        "/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1));
+            "/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1));
-    user_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        user_stop_sub_ = this->create_subscription<std_msgs::msg::Bool>(
-        "user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1));
+            "user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1));
+        scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+            "scan", 10, std::bind(&Ros2Aria::scan_callback, this, _1));
+    }
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
     clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
         "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
-    scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+
-        "scan", 10, std::bind(&Ros2Aria::scan_callback, this, _1));
 
     auto r = robot->getRobot();
     if(use_sonar){
@@ -59,8 +63,9 @@ Ros2Aria::Ros2Aria()
 
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
 
-    RCLCPP_INFO(get_logger(), "NAMESPACE = %s", get_namespace());
+    ros_namespace = get_namespace() == std::string("/")? "" : get_namespace();
-    RCLCPP_INFO(get_logger(), "ID = %d", id);
+    RCLCPP_INFO_STREAM(get_logger(), "NAMESPACE = " << ros_namespace);
+    RCLCPP_INFO_STREAM(get_logger(), "ID = " << id);
 
 }
 
@@ -70,7 +75,7 @@ std::size_t Ros2Aria::extract_number_from_string(const std::string& str) {
         if (isdigit(c)) {
             numericPart += c;
         } else if (!numericPart.empty()) {
-            break; 
+            break;
         }
     }
     int number;
@@ -81,11 +86,14 @@ std::size_t Ros2Aria::extract_number_from_string(const std::string& str) {
 void Ros2Aria::handle_parameters(){
     declare_parameter("use_sonar", true);
     declare_parameter("num_of_sonars", 0);
+    declare_parameter("use_safety_system", true);
     use_sonar = get_parameter("use_sonar").as_bool();
     num_of_sonars = get_parameter("num_of_sonars").as_int();
+    use_safety_system = get_parameter("use_safety_system").as_bool();
 
-    RCLCPP_INFO(get_logger(), "use_sonar = %d", use_sonar);
+    RCLCPP_INFO_STREAM(get_logger(), "use_sonar = " << use_sonar);
-    RCLCPP_INFO(get_logger(), "num_of_sonars = %d", num_of_sonars);
+    RCLCPP_INFO_STREAM(get_logger(), "num_of_sonars = " << num_of_sonars);
+    RCLCPP_INFO_STREAM(get_logger(), "use_safety_system = " << use_safety_system);
 }
 
 Ros2Aria::~Ros2Aria()

src/ros2aria/src/sonar.cpp

@@ -5,7 +5,7 @@ sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)
 {
     sensor_msgs::msg::PointCloud cloud; 
     cloud.header.stamp = stamp;
-    cloud.header.frame_id = std::string(get_namespace()) + "/front_sonar";
+    cloud.header.frame_id = ros_namespace + "/front_sonar";
 
     auto r = robot->getRobot();