dded footprint to Drive module for visualization

Signed-off-by: Alberto Tudela <[email protected]>

scitos2_modules/CHANGELOG.rst

@@ -8,4 +8,5 @@ Changelog for package scitos2_modules
 * Remove CMakeLists.mira and added dependency to scitos2_common.
 * Rename drive services.
 * Added unit testing.
+* Added footprint to Drive module for visualization.
 * Contributors: Alberto Tudela.

scitos2_modules/README.md

@@ -162,6 +162,14 @@ The Drive module controls the robot's motors, handling velocity commands, odomet
 
 	This parameter sets the interval in milliseconds to reset motor stop when the bumper is pressed. If set to 0, the motor stop will not be reset.
 
+* ** `footprint`** (string, default: "")
+
+	Specifies the list of points that define the footprint of the robot. The format is the same as the one used in the `nav2_costmap_2d` package.
+
+* ** `robot_radius`** (double, default: 0.5)
+
+	Specifies the radius of the robot in meters.
+
 ### EBC
 
 The EBC module controls the power for extra devices.

scitos2_modules/include/scitos2_modules/drive.hpp

@@ -261,6 +261,20 @@ class Drive : public scitos2_core::Module
    */
   visualization_msgs::msg::MarkerArray createBumperMarkers();
 
+/**
+ * @brief Create a circular footprint from a given radius
+ */
+  std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(double radius);
+
+/**
+ * @brief Make the footprint from the given string.
+ *
+ * Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
+ *
+ */
+  bool makeFootprintFromString(
+    const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint);
+
   /**
    * @brief Callback executed when a parameter change is detected.
    * @param event ParameterEvent message
@@ -367,6 +381,10 @@ class Drive : public scitos2_core::Module
   rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Time last_bumper_reset_;
   rclcpp::Duration reset_bumper_interval_{0, 0};
+  bool use_radius_{false};
+  std::string footprint_;
+  double robot_radius_;
+  std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
 
   // TF
   std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

scitos2_modules/src/drive.cpp

@@ -19,6 +19,7 @@
 
 // ROS
 #include "nav2_util/node_utils.hpp"
+#include "nav2_util/array_parser.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -128,8 +129,7 @@ void Drive::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, s
     .set__description("Publish the odometry as a tf2 transform"));
   node->get_parameter(plugin_name_ + ".publish_tf", publish_tf_);
   RCLCPP_INFO(
-    logger_, "The parameter publish_tf_ is set to: [%s]",
-    publish_tf_ ? "true" : "false");
+    logger_, "The parameter publish_tf_ is set to: [%s]", publish_tf_ ? "true" : "false");
 
   int rbi = 0;
   nav2_util::declare_parameter_if_not_declared(
@@ -143,6 +143,36 @@ void Drive::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, s
   set_mira_param(
     authority_, "MainControlUnit.RearLaser.Enabled", magnetic_barrier_enabled ? "true" : "false");
 
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".footprint",
+    rclcpp::ParameterValue(""), rcl_interfaces::msg::ParameterDescriptor()
+    .set__description("The footprint of the robot"));
+  node->get_parameter(plugin_name_ + ".footprint", footprint_);
+  RCLCPP_INFO(logger_, "The parameter footprint is set to: [%s]", footprint_);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, plugin_name_ + ".robot_radius",
+    rclcpp::ParameterValue(0.5), rcl_interfaces::msg::ParameterDescriptor()
+    .set__description("The radius of the robot"));
+  node->get_parameter(plugin_name_ + ".robot_radius", robot_radius_);
+  RCLCPP_INFO(logger_, "The parameter robot_radius is set to: [%f]", robot_radius_);
+
+  // If the footprint has been specified, it must be in the correct format
+  use_radius_ = true;
+  if (footprint_ != "" && footprint_ != "[]") {
+    // Footprint parameter has been specified, try to convert it
+    if (makeFootprintFromString(footprint_, unpadded_footprint_)) {
+      // The specified footprint is valid, so we'll use that instead of the radius
+      use_radius_ = false;
+    } else {
+      // Footprint provided but invalid, so stay with the radius
+      unpadded_footprint_ = makeFootprintFromRadius(robot_radius_);
+      RCLCPP_ERROR(
+        logger_, "The footprint parameter is invalid: \"%s\", using radius (%lf) instead",
+        footprint_.c_str(), robot_radius_);
+    }
+  }
+
   // Callback for monitor changes in parameters
   dyn_params_handler_ = node->add_on_set_parameters_callback(
     std::bind(&Drive::dynamicParametersCallback, this, _1));
@@ -411,37 +441,74 @@ visualization_msgs::msg::MarkerArray Drive::createBumperMarkers()
     cylinder.color.a = 1.0;
   }
 
-  // Left bumper
-  cylinder.id = 0;
-  cylinder.pose.position.x = -0.10;
-  cylinder.pose.position.y = 0.20;
-  cylinder.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 1, 0}, M_PI / 2.0));
-  markers_msg.markers.push_back(cylinder);
-
-  // Right bumper
-  cylinder.id = 1;
-  cylinder.pose.position.x = -0.10;
-  cylinder.pose.position.y = -0.20;
-  cylinder.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 1, 0}, M_PI / 2.0));
-  markers_msg.markers.push_back(cylinder);
-
-  // Front bumper
-  cylinder.id = 2;
-  cylinder.pose.position.y = 0.0;
-  cylinder.pose.position.x = 0.15;
-  cylinder.pose.orientation = tf2::toMsg(tf2::Quaternion({1, 0, 0}, M_PI / 2.0));
-  markers_msg.markers.push_back(cylinder);
-
-  // Rear bumper
-  cylinder.id = 3;
-  cylinder.pose.position.y = 0.0;
-  cylinder.pose.position.x = -0.35;
-  cylinder.pose.orientation = tf2::toMsg(tf2::Quaternion({1, 0, 0}, M_PI / 2.0));
-  markers_msg.markers.push_back(cylinder);
+  // Create the markers using the footprint
+  for (size_t i = 0; i < unpadded_footprint_.size(); ++i) {
+    cylinder.id = i + 4;
+    cylinder.pose.position.x = unpadded_footprint_[i].x;
+    cylinder.pose.position.y = unpadded_footprint_[i].y;
+    markers_msg.markers.push_back(cylinder);
+  }
 
   return markers_msg;
 }
 
+std::vector<geometry_msgs::msg::Point> Drive::makeFootprintFromRadius(double radius)
+{
+  std::vector<geometry_msgs::msg::Point> points;
+
+  // Loop over 16 angles around a circle making a point each time
+  int N = 16;
+  geometry_msgs::msg::Point pt;
+  for (int i = 0; i < N; ++i) {
+    double angle = i * 2 * M_PI / N;
+    pt.x = cos(angle) * radius;
+    pt.y = sin(angle) * radius;
+
+    points.push_back(pt);
+  }
+
+  return points;
+}
+
+bool Drive::makeFootprintFromString(
+  const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint)
+{
+  std::string error;
+  std::vector<std::vector<float>> vvf = nav2_util::parseVVF(footprint_string, error);
+
+  if (error != "") {
+    RCLCPP_ERROR(logger_, "Error parsing footprint parameter: '%s'", error.c_str());
+    RCLCPP_ERROR(logger_, "Footprint string was '%s'.", footprint_string.c_str());
+    return false;
+  }
+
+  // Convert vvf into points.
+  if (vvf.size() < 3) {
+    RCLCPP_ERROR(
+      logger_,
+      "You must specify at least three points for the robot footprint, reverting to previous footprint."); //NOLINT
+    return false;
+  }
+  footprint.reserve(vvf.size());
+  for (unsigned int i = 0; i < vvf.size(); i++) {
+    if (vvf[i].size() == 2) {
+      geometry_msgs::msg::Point point;
+      point.x = vvf[i][0];
+      point.y = vvf[i][1];
+      point.z = 0;
+      footprint.push_back(point);
+    } else {
+      RCLCPP_ERROR(
+        logger_,
+        "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT
+        static_cast<int>(vvf[i].size()));
+      return false;
+    }
+  }
+
+  return true;
+}
+
 bool Drive::isEmergencyStopReleased(scitos2_msgs::msg::EmergencyStopStatus msg)
 {
   return msg.emergency_stop_activated && !msg.emergency_stop_status;

scitos2_modules/test/test_drive.cpp

@@ -65,6 +65,17 @@ class DriveFixture : public scitos2_modules::Drive
     emergency_stop_activated_ = false;
   }
 
+  std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius(double radius)
+  {
+    return scitos2_modules::Drive::makeFootprintFromRadius(radius);
+  }
+
+  bool makeFootprintFromString(
+    const std::string & footprint_string, std::vector<geometry_msgs::msg::Point> & footprint)
+  {
+    return scitos2_modules::Drive::makeFootprintFromString(footprint_string, footprint);
+  }
+
   visualization_msgs::msg::MarkerArray createBumperMarkers()
   {
     return scitos2_modules::Drive::createBumperMarkers();
@@ -131,6 +142,18 @@ TEST(ScitosDriveTest, configure) {
   // Cleaning up
   module->deactivate();
   module->cleanup();
+
+  // Now, we set an invalid footprint to show the error
+  node->set_parameter(rclcpp::Parameter("test.footprint", "[[bad_string"));
+
+  // Configure the module
+  module->configure(node, "test");
+  module->activate();
+
+  // Cleaning up
+  module->deactivate();
+  module->cleanup();
+
   rclcpp::shutdown();
 }
 
@@ -449,9 +472,60 @@ TEST(ScitosDriveTest, suspendBumper) {
   rclcpp::shutdown();
 }
 
+TEST(ScitosDriveTest, makeFootprintFromRadius) {
+  // Create the module
+  auto module = std::make_shared<DriveFixture>();
+
+  // Set a valid radius
+  auto footprint = module->makeFootprintFromRadius(0.5);
+
+  // Check results
+  EXPECT_EQ(16u, footprint.size());
+}
+
+TEST(ScitosDriveTest, makeFootprintFromString) {
+  // Create the module
+  auto module = std::make_shared<DriveFixture>();
+
+  // Set a valid footprint
+  std::vector<geometry_msgs::msg::Point> footprint;
+  bool result = module->makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint);
+  EXPECT_EQ(result, true);
+  EXPECT_EQ(4u, footprint.size());
+  EXPECT_NEAR(footprint[0].x, 1.0, 1e-5);
+  EXPECT_NEAR(footprint[0].y, 2.2, 1e-5);
+  EXPECT_NEAR(footprint[1].x, 0.3, 1e-5);
+  EXPECT_NEAR(footprint[1].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[2].x, -0.3, 1e-5);
+  EXPECT_NEAR(footprint[2].y, -4e4, 1e-5);
+  EXPECT_NEAR(footprint[3].x, -1.0, 1e-5);
+  EXPECT_NEAR(footprint[3].y, 2.2, 1e-5);
+
+  // Set a wrong string
+  result = module->makeFootprintFromString(
+    "[[bad_string", footprint);
+  EXPECT_EQ(result, false);
+
+  // Set two points
+  result = module->makeFootprintFromString("[[1, 2.2], [.3, -4e4]]", footprint);
+  EXPECT_EQ(result, false);
+
+  // Set a wrong number of points
+  result = module->makeFootprintFromString(
+    "[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint);
+  EXPECT_EQ(result, false);
+}
+
 TEST(ScitosDriveTest, createBumperMarkers) {
   rclcpp::init(0, nullptr);
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testDrive");
+
+  // Set the footprint
+  nav2_util::declare_parameter_if_not_declared(
+    node, "test.footprint",
+    rclcpp::ParameterValue("[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]"));
+
   // Create the module
   auto module = std::make_shared<DriveFixture>();
   module->configure(node, "test");

scitos2_modules/test/test_ebc.cpp

@@ -26,6 +26,7 @@ class EBCFixture : public scitos2_modules::EBC
 };
 
 TEST(ScitosEBCTest, configure) {
+  rclcpp::init(0, nullptr);
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testEBC");
 
   // Create the module
@@ -36,9 +37,11 @@ TEST(ScitosEBCTest, configure) {
   // Cleaning up
   module->deactivate();
   module->cleanup();
+  rclcpp::shutdown();
 }
 
 TEST(ScitosEBCTest, dynamicParameters) {
+  rclcpp::init(0, nullptr);
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testEBC");
 
   // Create the module
@@ -98,10 +101,12 @@ TEST(ScitosEBCTest, dynamicParameters) {
   // Cleaning up
   module->deactivate();
   module->cleanup();
+  rclcpp::shutdown();
 }
 
 
 TEST(ScitosEBCTest, dynamicParametersOutRange) {
+  rclcpp::init(0, nullptr);
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testEBC");
 
   // Create the module
@@ -161,15 +166,14 @@ TEST(ScitosEBCTest, dynamicParametersOutRange) {
   // Cleaning up
   module->deactivate();
   module->cleanup();
+  rclcpp::shutdown();
 }
 
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);
-  rclcpp::init(argc, argv);
-  mira::Framework framework(argc, argv);
+  mira::Framework framework(0, nullptr);
   framework.start();
   bool success = RUN_ALL_TESTS();
-  rclcpp::shutdown();
   return success;
 }