publish motion target as pose (#4839)

Signed-off-by: Michael Ferguson <[email protected]>

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -173,7 +173,7 @@ class GracefulController : public nav2_core::Controller
 
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_plan_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> local_plan_pub_;
-  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseStamped>>
   motion_target_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>>
   slowdown_pub_;

nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp

@@ -15,22 +15,11 @@
 #ifndef NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_
 #define NAV2_GRACEFUL_CONTROLLER__UTILS_HPP_
 
-#include "geometry_msgs/msg/point_stamped.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 
 namespace nav2_graceful_controller
 {
-/**
-   * @brief Create a PointStamped message of the motion target for
-   * debugging / visualization porpuses.
-   *
-   * @param motion_target Motion target in PoseStamped format
-   * @return geometry_msgs::msg::PointStamped Motion target in PointStamped format
-   */
-geometry_msgs::msg::PointStamped createMotionTargetMsg(
-  const geometry_msgs::msg::PoseStamped & motion_target);
-
 /**
    * @brief Create a flat circle marker of radius slowdown_radius around the motion target for
    * debugging / visualization porpuses.

nav2_graceful_controller/src/graceful_controller.cpp

@@ -61,7 +61,7 @@ void GracefulController::configure(
   // Publishers
   transformed_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
   local_plan_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
-  motion_target_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("motion_target", 1);
+  motion_target_pub_ = node->create_publisher<geometry_msgs::msg::PoseStamped>("motion_target", 1);
   slowdown_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("slowdown", 1);
 
   RCLCPP_INFO(logger_, "Configured Graceful Motion Controller: %s", plugin_name_.c_str());
@@ -233,8 +233,7 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
       // Successfully simulated to target_pose - compute velocity at this moment
       // Publish the selected target_pose
-      auto motion_target_point = nav2_graceful_controller::createMotionTargetMsg(target_pose);
-      motion_target_pub_->publish(motion_target_point);
+      motion_target_pub_->publish(target_pose);
       // Publish marker for slowdown radius around motion target for debugging / visualization
       auto slowdown_marker = nav2_graceful_controller::createSlowdownMarker(
         target_pose, params_->slowdown_radius);

nav2_graceful_controller/src/utils.cpp

@@ -17,16 +17,6 @@
 namespace nav2_graceful_controller
 {
 
-geometry_msgs::msg::PointStamped createMotionTargetMsg(
-  const geometry_msgs::msg::PoseStamped & motion_target)
-{
-  geometry_msgs::msg::PointStamped motion_target_point;
-  motion_target_point.header = motion_target.header;
-  motion_target_point.point = motion_target.pose.position;
-  motion_target_point.point.z = 0.01;
-  return motion_target_point;
-}
-
 visualization_msgs::msg::Marker createSlowdownMarker(
   const geometry_msgs::msg::PoseStamped & motion_target, const double & slowdown_radius)
 {

nav2_graceful_controller/test/test_graceful_controller.cpp

@@ -59,12 +59,6 @@ class GMControllerFixture : public nav2_graceful_controller::GracefulController
 
   nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();}
 
-  geometry_msgs::msg::PointStamped createMotionTargetMsg(
-    const geometry_msgs::msg::PoseStamped & motion_target)
-  {
-    return nav2_graceful_controller::createMotionTargetMsg(motion_target);
-  }
-
   visualization_msgs::msg::Marker createSlowdownMarker(
     const geometry_msgs::msg::PoseStamped & motion_target)
   {
@@ -345,34 +339,6 @@ TEST(GracefulControllerTest, dynamicParameters) {
   EXPECT_EQ(controller->getAllowBackward(), false);
 }
 
-TEST(GracefulControllerTest, createMotionTargetMsg) {
-  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testGraceful");
-  auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
-  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
-
-  // Create controller
-  auto controller = std::make_shared<GMControllerFixture>();
-  costmap_ros->on_configure(rclcpp_lifecycle::State());
-  controller->configure(node, "test", tf, costmap_ros);
-  controller->activate();
-
-  // Create motion target
-  geometry_msgs::msg::PoseStamped motion_target;
-  motion_target.header.frame_id = "map";
-  motion_target.pose.position.x = 1.0;
-  motion_target.pose.position.y = 2.0;
-  motion_target.pose.orientation = tf2::toMsg(tf2::Quaternion({0, 0, 1}, 0.0));
-
-  // Create motion target message
-  auto motion_target_msg = controller->createMotionTargetMsg(motion_target);
-
-  // Check results
-  EXPECT_EQ(motion_target_msg.header.frame_id, "map");
-  EXPECT_EQ(motion_target_msg.point.x, 1.0);
-  EXPECT_EQ(motion_target_msg.point.y, 2.0);
-  EXPECT_EQ(motion_target_msg.point.z, 0.01);
-}
-
 TEST(GracefulControllerTest, createSlowdownMsg) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testGraceful");
   auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());