AGVFM-537 AGV Naviation: create logic to calculate the length for w2mo path using SMAC (#82)

sofiasemianchuk · doisyg · web-flow · commit d755910e0c0d · 2021-08-19T13:14:48.000+03:00
* Ability to get a path from the planner from any 2 poses (start_pose and goal): add use_start_pose and start_pose to ComputePathToPose action (ros-navigation#2179) * add use_start_pose to ComputePathToPose.action * add use_start_pose to ComputePathToPose bt * start instead of start_pose + else if * [ComputePathToPose Action API break] change 'pose' for 'goal' +use_start * test wip * add compute path to pose BT test for use_start * last start_pose renaming * Transform start and goal in costmap frame * Revert "Transform start and goal in costmap frame" This reverts commit 4ed8eb0. * lint * AGVFM-537 AGV Naviation: create logic to calculate the length for w2mo path using SMAC -> fixed incorrect cherry pick, fixed compute_path_to_pose_python_action.cpp; * Fix ros-navigation#2186 (ros-navigation#2222) * Fix ros-navigation#2186 * goal_pose instead of goal->goal * avoid redundant code * change name and switch to input + output param pattern * code style * Fix ros-navigation#2186 (ros-navigation#2222) * Fix ros-navigation#2186 * goal_pose instead of goal->goal * avoid redundant code * change name and switch to input + output param pattern * code style * fixed comments; * Revert "fixed comments;" This reverts commit 412464b1 Co-authored-by: G.Doisy <doisyg@users.noreply.github.com>
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
@@ -42,6 +42,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
       {
         BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
         BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
+        BT::InputPort<geometry_msgs::msg::PoseStamped>(
+          "start", "Start pose of the path if overriding current robot pose"),
         BT::InputPort<std::string>("planner_id", ""),
       });
   }
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -18,6 +18,7 @@
 
     <Action ID="ComputePathToPose">
       <input_port name="goal">Destination to plan to</input_port>
+      <input_port name="start">Start pose of the path if overriding current robot pose</input_port>
       <output_port name="path">Path created by ComputePathToPose node</output_port>
       <input_port name="planner_id"/>
     </Action>
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
@@ -30,8 +30,11 @@ ComputePathToPoseAction::ComputePathToPoseAction(
 
 void ComputePathToPoseAction::on_tick()
 {
-  getInput("goal", goal_.pose);
+  getInput("goal", goal_.goal);
   getInput("planner_id", goal_.planner_id);
+  if (getInput("start", goal_.start)) {
+    goal_.use_start = true;
+  }
 }
 
 BT::NodeStatus ComputePathToPoseAction::on_success()
diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_python_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_python_action.cpp
@@ -31,7 +31,7 @@ namespace nav2_behavior_tree
 
     void ComputePathToPoseActionPython::on_tick()
     {
-        getInput("goal", goal_.pose);
+        getInput("goal", goal_.goal);
         getInput("planner_id", goal_.planner_id);
     }
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp
@@ -41,8 +41,13 @@ class ComputePathToPoseActionServer : public TestActionServer<nav2_msgs::action:
   {
     const auto goal = goal_handle->get_goal();
     auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
-    result->path.poses.resize(1);
-    result->path.poses[0].pose.position.x = goal->pose.pose.position.x;
+    result->path.poses.resize(2);
+    result->path.poses[1].pose.position.x = goal->goal.pose.position.x;
+    if (goal->use_start) {
+      result->path.poses[0].pose.position.x = goal->start.pose.position.x;
+    } else {
+      result->path.poses[0].pose.position.x = 0.0;
+    }
     goal_handle->succeed(result);
   }
 };
@@ -133,7 +138,8 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick)
   EXPECT_EQ(config_->blackboard->get<bool>("path_updated"), false);
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
   EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
-  EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0);
+  EXPECT_FALSE(action_server_->getCurrentGoal()->use_start);
   EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
 
   // tick until node succeeds
@@ -147,8 +153,9 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick)
   // check if returned path is correct
   nav_msgs::msg::Path path;
   config_->blackboard->get<nav_msgs::msg::Path>("path", path);
-  EXPECT_EQ(path.poses.size(), 1u);
-  EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
 
   // halt node so another goal can be sent
   tree_->rootNode()->halt();
@@ -159,18 +166,89 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick)
   config_->blackboard->set("goal", goal);
 
   EXPECT_EQ(tree_->rootNode()->executeTick(), BT::NodeStatus::RUNNING);
-  EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5);
 
   while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
     tree_->rootNode()->executeTick();
   }
 
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5);
   // path is updated on new goal
   EXPECT_EQ(config_->blackboard->get<bool>("path_updated"), true);
 
   config_->blackboard->get<nav_msgs::msg::Path>("path", path);
-  EXPECT_EQ(path.poses.size(), 1u);
-  EXPECT_EQ(path.poses[0].pose.position.x, -2.5);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 0.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
+}
+
+TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start)
+{
+  // create tree
+  std::string xml_txt =
+    R"(
+      <root main_tree_to_execute = "MainTree" >
+        <BehaviorTree ID="MainTree">
+            <ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new start and set it on blackboard
+  geometry_msgs::msg::PoseStamped start;
+  start.header.stamp = node_->now();
+  start.pose.position.x = 2.0;
+  config_->blackboard->set("start", start);
+
+  // create new goal and set it on blackboard
+  geometry_msgs::msg::PoseStamped goal;
+  goal.header.stamp = node_->now();
+  goal.pose.position.x = 1.0;
+  config_->blackboard->set("goal", goal);
+
+  // tick until node succeeds
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  // the goal should have reached our server
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(tree_->rootNode()->getInput<std::string>("planner_id"), std::string("GridBased"));
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0);
+  EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0);
+  EXPECT_TRUE(action_server_->getCurrentGoal()->use_start);
+  EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased"));
+
+  // check if returned path is correct
+  nav_msgs::msg::Path path;
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, 2.0);
+  EXPECT_EQ(path.poses[1].pose.position.x, 1.0);
+
+  // halt node so another goal can be sent
+  tree_->rootNode()->halt();
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE);
+
+  // set new goal and new start
+  goal.pose.position.x = -2.5;
+  start.pose.position.x = -1.5;
+  config_->blackboard->set("goal", goal);
+  config_->blackboard->set("start", start);
+
+  while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) {
+    tree_->rootNode()->executeTick();
+  }
+
+  EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS);
+  EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5);
+
+  config_->blackboard->get<nav_msgs::msg::Path>("path", path);
+  EXPECT_EQ(path.poses.size(), 2u);
+  EXPECT_EQ(path.poses[0].pose.position.x, -1.5);
+  EXPECT_EQ(path.poses[1].pose.position.x, -2.5);
 }
 
 int main(int argc, char ** argv)
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp
@@ -140,6 +140,16 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose);
 
+  /**
+   * @brief Transform the input_pose in the global frame of the costmap
+   * @param input_pose pose to be transformed
+   * @param transformed_pose pose transformed
+   * @return True if the pose was transformed successfully, false otherwise
+   */
+  bool transformPoseToGlobalFrame(
+    const geometry_msgs::msg::PoseStamped & input_pose,
+    geometry_msgs::msg::PoseStamped & transformed_pose);
+
   /** @brief Returns costmap name */
   std::string getName() const
   {
diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp
@@ -548,6 +548,21 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
   }
 }
 
+bool
+Costmap2DROS::transformPoseToGlobalFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose)
+{
+  if (input_pose.header.frame_id == global_frame_) {
+    transformed_pose = input_pose;
+    return true;
+  } else {
+    return nav2_util::transformPoseInTargetFrame(
+      input_pose, transformed_pose, *tf_buffer_,
+      global_frame_, transform_tolerance_);
+  }
+}
+
 bool
 Costmap2DROS::is_static_layer_costmap_initialized()
 {
diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action
@@ -1,6 +1,8 @@
 #goal definition
-geometry_msgs/PoseStamped pose
+geometry_msgs/PoseStamped goal
+geometry_msgs/PoseStamped start
 string planner_id
+bool use_start # If true, use current robot pose as path start, if false, use start above instead
 ---
 #result definition
 nav_msgs/Path path
diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp
@@ -222,32 +222,46 @@ PlannerServer::computePlan()
     }
 
     geometry_msgs::msg::PoseStamped start;
-    if (!costmap_ros_->getRobotPose(start)) {
+    if (goal->use_start) {
+      start = goal->start;
+    } else if (!costmap_ros_->getRobotPose(start)) {
       error_publisher_.publish("Couldn't get robot current position", 27);
       action_server_->terminate_current();
       return;
     }
 
+    // Changing the start and goal pose frame to the global_frame_ of costmap_ros_ if needed
+    geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
+    geometry_msgs::msg::PoseStamped start_pose = start;
+    if (!costmap_ros_->transformPoseToGlobalFrame(start, start_pose) ||
+      !costmap_ros_->transformPoseToGlobalFrame(goal->goal, goal_pose))
+    {
+      RCLCPP_WARN(
+        get_logger(), "Could not transform the start or goal pose in the costmap frame");
+      action_server_->terminate_current();
+      return;
+    }
+
     if (action_server_->is_preempt_requested()) {
       goal = action_server_->accept_pending_goal();
     }
 
-    result->path = getPlan(start, goal->pose, goal->planner_id);
+    result->path = getPlan(start_pose, goal_pose, goal->planner_id);
 
     if (result->path.poses.size() == 0) {
       RCLCPP_WARN(
         get_logger(), "Planning algorithm %s failed to generate a valid"
         " path from (%.2f, %.2f) to (%.2f, %.2f)", goal->planner_id.c_str(),
-        start.pose.position.x, start.pose.position.y, goal->pose.pose.position.x, goal->pose.pose.position.y);
+        start_pose.pose.position.x, start_pose.pose.position.y, goal_pose.pose.position.x, goal_pose.pose.position.y);
       action_server_->terminate_current();
       return;
     }
 
     RCLCPP_DEBUG(
       get_logger(),
-      "Found valid path of size %u to (%.2f, %.2f)",
-      result->path.poses.size(), goal->pose.pose.position.x,
-      goal->pose.pose.position.y);
+      "Found valid path of size %lu to (%.2f, %.2f)",
+      result->path.poses.size(), goal_pose.pose.position.x,
+      goal_pose.pose.position.y);
 
     // Publish the plan for visualization purposes
     publishPlan(result->path);
@@ -266,8 +280,8 @@ PlannerServer::computePlan()
   } catch (std::exception & ex) {
     RCLCPP_WARN(
       get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"",
-      goal->planner_id.c_str(), goal->pose.pose.position.x,
-      goal->pose.pose.position.y, ex.what());
+      goal->planner_id.c_str(), goal->goal.pose.position.x,
+      goal->goal.pose.position.y, ex.what());
       error_publisher_.publish(goal->planner_id + " plugin failed to plan calculation: " + ex.what(), 25);
     // TODO(orduno): provide information about fail error to parent task,
     //               for example: couldn't get costmap update
diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp
@@ -36,6 +36,12 @@ bool getCurrentPose(
   tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
   const std::string robot_frame = "base_link", const double transform_timeout = 0.1);
 
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout = 0.1);
+
 }  // end namespace nav2_util
 
 #endif  // NAV2_UTIL__ROBOT_UTILS_HPP_
diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp
@@ -28,39 +28,47 @@ bool getCurrentPose(
   tf2_ros::Buffer & tf_buffer, const std::string global_frame,
   const std::string robot_frame, const double transform_timeout)
 {
-  static rclcpp::Logger logger = rclcpp::get_logger("getCurrentPose");
-  geometry_msgs::msg::PoseStamped robot_pose;
-
   tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
-  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
-  robot_pose.header.frame_id = robot_frame;
-  robot_pose.header.stamp = rclcpp::Time();
+  global_pose.header.frame_id = robot_frame;
+  global_pose.header.stamp = rclcpp::Time();
+
+  return transformPoseInTargetFrame(
+    global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
+}
+
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout)
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
 
   try {
-    global_pose = tf_buffer.transform(
-      robot_pose, global_frame,
+    transformed_pose = tf_buffer.transform(
+      input_pose, target_frame,
       tf2::durationFromSec(transform_timeout));
     return true;
   } catch (tf2::LookupException & ex) {
     RCLCPP_ERROR(
       logger,
-      "No Transform available Error looking up robot pose: %s\n", ex.what());
+      "No Transform available Error looking up target frame: %s\n", ex.what());
   } catch (tf2::ConnectivityException & ex) {
     RCLCPP_ERROR(
       logger,
-      "Connectivity Error looking up robot pose: %s\n", ex.what());
+      "Connectivity Error looking up target frame: %s\n", ex.what());
   } catch (tf2::ExtrapolationException & ex) {
     RCLCPP_ERROR(
       logger,
-      "Extrapolation Error looking up robot pose: %s\n", ex.what());
+      "Extrapolation Error looking up target frame: %s\n", ex.what());
   } catch (tf2::TimeoutException & ex) {
     RCLCPP_ERROR(
       logger,
       "Transform timeout with tolerance: %.4f", transform_timeout);
   } catch (tf2::TransformException & ex) {
     RCLCPP_ERROR(
       logger, "Failed to transform from %s to %s",
-      global_frame.c_str(), robot_frame.c_str());
+      input_pose.header.frame_id.c_str(), target_frame.c_str());
   }
 
   return false;
diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp
@@ -29,4 +29,6 @@ TEST(RobotUtils, LookupExceptionError)
   geometry_msgs::msg::PoseStamped global_pose;
   tf2_ros::Buffer tf(node->get_clock());
   ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1));
+  global_pose.header.frame_id = "base_link";
+  ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1));
 }

Original file line number	Diff line number	Diff line change
`@@ -42,6 +42,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa`
`42`	`42`	`{`
`43`	`43`	`BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),`
`44`	`44`	`BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),`
	`45`	`+ BT::InputPort<geometry_msgs::msg::PoseStamped>(`
	`46`	`+ "start", "Start pose of the path if overriding current robot pose"),`
`45`	`47`	`BT::InputPort<std::string>("planner_id", ""),`
`46`	`48`	`});`
`47`	`49`	`}`
Original file line number	Diff line number	Diff line change
`@@ -30,8 +30,11 @@ ComputePathToPoseAction::ComputePathToPoseAction(`
`30`	`30`
`31`	`31`	`void ComputePathToPoseAction::on_tick()`
`32`	`32`	`{`
`33`		`- getInput("goal", goal_.pose);`
	`33`	`+ getInput("goal", goal_.goal);`
`34`	`34`	`getInput("planner_id", goal_.planner_id);`
	`35`	`+ if (getInput("start", goal_.start)) {`
	`36`	`+ goal_.use_start = true;`
	`37`	`+ }`
`35`	`38`	`}`
`36`	`39`
`37`	`40`	`BT::NodeStatus ComputePathToPoseAction::on_success()`
Original file line number	Diff line number	Diff line change
`@@ -31,7 +31,7 @@ namespace nav2_behavior_tree`
`31`	`31`
`32`	`32`	`void ComputePathToPoseActionPython::on_tick()`
`33`	`33`	`{`
`34`		`- getInput("goal", goal_.pose);`
	`34`	`+ getInput("goal", goal_.goal);`
`35`	`35`	`getInput("planner_id", goal_.planner_id);`
`36`	`36`	`}`
`37`	`37`