From dabd5bd871e5d297ebc37015baee6d1583f03fb7 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 31 Oct 2023 15:33:23 +0100 Subject: [PATCH] added use_safety_system parameter Signed-off-by: Jakub Delicat --- compose.ros2aria.yaml | 4 +-- src/ros2aria/include/ros2aria/ros2aria.hpp | 4 ++- src/ros2aria/src/cmd_vel.cpp | 22 ++++++------ src/ros2aria/src/pose.cpp | 4 +-- src/ros2aria/src/ros2aria.cpp | 40 +++++++++++++--------- src/ros2aria/src/sonar.cpp | 2 +- 6 files changed, 44 insertions(+), 32 deletions(-) diff --git a/compose.ros2aria.yaml b/compose.ros2aria.yaml index d718a11..d7c47ea 100644 --- a/compose.ros2aria.yaml +++ b/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 diff --git a/src/ros2aria/include/ros2aria/ros2aria.hpp b/src/ros2aria/include/ros2aria/ros2aria.hpp index d6b937c..e5b9149 100644 --- a/src/ros2aria/include/ros2aria/ros2aria.hpp +++ b/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 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); diff --git a/src/ros2aria/src/cmd_vel.cpp b/src/ros2aria/src/cmd_vel.cpp index 7876e9e..447db1f 100644 --- a/src/ros2aria/src/cmd_vel.cpp +++ b/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){ - x = 0.0; - y = 0.0; - z = 0.0; - } - else{ - // apply limits - x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x; - 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; + if(use_safety_system){ + if (master_stop or obstacle_too_close or user_stop){ + x = 0.0; + y = 0.0; + z = 0.0; + } + else{ + // apply limits + x = std::abs(x) > restrictions.linear_velocity.data ? std::abs(x)/x * restrictions.linear_velocity.data : x; + 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; diff --git a/src/ros2aria/src/pose.cpp b/src/ros2aria/src/pose.cpp index 6387dce..5790f54 100644 --- a/src/ros2aria/src/pose.cpp +++ b/src/ros2aria/src/pose.cpp @@ -19,8 +19,8 @@ std::pair 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.child_frame_id = std::string(get_namespace()) + "/base_link"; + odom_msg.header.frame_id = ros_namespace + "/odom"; + odom_msg.child_frame_id = ros_namespace + "/base_link"; odom_msg.header.stamp = stamp; geometry_msgs::msg::TransformStamped transform; diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 10942ed..9152fee 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/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(this, "/dev/ttyS0"); handle_parameters(); RCLCPP_INFO(this->get_logger(), "starting subscribers and services"); - restrictions_sub_ = this->create_subscription( - "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1)); - init_restrictions(); - - master_stop_sub_ = this->create_subscription( - "/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1)); - user_stop_sub_ = this->create_subscription( - "user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1)); + if(use_safety_system){ + restrictions_sub_ = this->create_subscription( + "/pioneers/restrictions", 10, std::bind(&Ros2Aria::restrictions_callback, this, _1)); + init_restrictions(); + master_stop_sub_ = this->create_subscription( + "/pioneers/master_stop", 10, std::bind(&Ros2Aria::master_stop_callback, this, _1)); + user_stop_sub_ = this->create_subscription( + "user_stop", 10, std::bind(&Ros2Aria::user_stop_callback, this, _1)); + scan_sub_ = this->create_subscription( + "scan", 10, std::bind(&Ros2Aria::scan_callback, this, _1)); + } cmd_vel_sub_ = this->create_subscription( "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1)); clutch_sub_ = this->create_subscription( "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1)); - scan_sub_ = this->create_subscription( - "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()); - RCLCPP_INFO(get_logger(), "ID = %d", id); + ros_namespace = get_namespace() == std::string("/")? "" : get_namespace(); + 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(get_logger(), "num_of_sonars = %d", num_of_sonars); + RCLCPP_INFO_STREAM(get_logger(), "use_sonar = " << use_sonar); + RCLCPP_INFO_STREAM(get_logger(), "num_of_sonars = " << num_of_sonars); + RCLCPP_INFO_STREAM(get_logger(), "use_safety_system = " << use_safety_system); } Ros2Aria::~Ros2Aria() diff --git a/src/ros2aria/src/sonar.cpp b/src/ros2aria/src/sonar.cpp index 16aefe1..93820e0 100644 --- a/src/ros2aria/src/sonar.cpp +++ b/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();