diff --git a/scitos2_modules/CHANGELOG.rst b/scitos2_modules/CHANGELOG.rst index 2969f3d..98f6179 100644 --- a/scitos2_modules/CHANGELOG.rst +++ b/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. diff --git a/scitos2_modules/README.md b/scitos2_modules/README.md index ba529aa..3d1aedb 100644 --- a/scitos2_modules/README.md +++ b/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. diff --git a/scitos2_modules/include/scitos2_modules/drive.hpp b/scitos2_modules/include/scitos2_modules/drive.hpp index e311fac..7112f00 100644 --- a/scitos2_modules/include/scitos2_modules/drive.hpp +++ b/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 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 & 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 unpadded_footprint_; // TF std::unique_ptr tf_broadcaster_; diff --git a/scitos2_modules/src/drive.cpp b/scitos2_modules/src/drive.cpp index 3c4d6c0..129056b 100644 --- a/scitos2_modules/src/drive.cpp +++ b/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 Drive::makeFootprintFromRadius(double radius) +{ + std::vector 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 & footprint) +{ + std::string error; + std::vector> 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(vvf[i].size())); + return false; + } + } + + return true; +} + bool Drive::isEmergencyStopReleased(scitos2_msgs::msg::EmergencyStopStatus msg) { return msg.emergency_stop_activated && !msg.emergency_stop_status; diff --git a/scitos2_modules/test/test_drive.cpp b/scitos2_modules/test/test_drive.cpp index eb426a3..6551681 100644 --- a/scitos2_modules/test/test_drive.cpp +++ b/scitos2_modules/test/test_drive.cpp @@ -65,6 +65,17 @@ class DriveFixture : public scitos2_modules::Drive emergency_stop_activated_ = false; } + std::vector makeFootprintFromRadius(double radius) + { + return scitos2_modules::Drive::makeFootprintFromRadius(radius); + } + + bool makeFootprintFromString( + const std::string & footprint_string, std::vector & 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(); + + // 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(); + + // Set a valid footprint + std::vector 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("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(); module->configure(node, "test"); diff --git a/scitos2_modules/test/test_ebc.cpp b/scitos2_modules/test/test_ebc.cpp index 0e135c5..38153ba 100644 --- a/scitos2_modules/test/test_ebc.cpp +++ b/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("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("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("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; }