update rotate to goal

config/pnkx_local_planner_params.yaml

@@ -184,4 +184,4 @@ GoalChecker:
   library_path: libmkt_plugins_goal_checker
 
 SimpleGoalChecker:
-  library_path: libmkt_plugins_simple_goal_checker
+  library_path: libmkt_plugins_goal_checker

src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt

@@ -33,6 +33,8 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_core
     robot_geometry_msgs
     robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
     tf3
     robot_tf3_geometry_msgs
     robot_time
@@ -49,6 +51,8 @@ else()
     robot_nav_core
     robot_geometry_msgs
     robot_nav_msgs
+    robot_nav_2d_msgs
+    robot_nav_2d_utils
     tf3
     robot_tf3_geometry_msgs
     robot_time

src/Algorithms/Packages/global_planners/two_points_planner/package.xml

@@ -31,6 +31,12 @@
   <build_depend>robot_nav_msgs</build_depend>
   <run_depend>robot_nav_msgs</run_depend>
 
+  <build_depend>robot_nav_2d_msgs</build_depend>
+  <run_depend>robot_nav_2d_msgs</run_depend>
+
+  <build_depend>robot_nav_2d_utils</build_depend>
+  <run_depend>robot_nav_2d_utils</run_depend>
+
   <build_depend>tf3</build_depend>
   <run_depend>tf3</run_depend>
 

src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp

@@ -3,6 +3,8 @@
 #include <robot_costmap_2d/inflation_layer.h>
 #include <boost/dll/alias.hpp>
 #include <robot_nav_msgs/Path.h>
+#include <robot_nav_2d_msgs/Pose2DStamped.h>
+#include <robot_nav_2d_utils/conversions.h>
 #include <tf3/LinearMath/Quaternion.h>
 #include <tf3/utils.h>
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
@@ -109,6 +111,10 @@ namespace two_points_planner
       robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__);
       return false;
     }
+    robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start);
+    robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal);
+    robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", 
+      start_2d.pose.x, start_2d.pose.y, start_2d.pose.theta, goal_2d.pose.x, goal_2d.pose.y, goal_2d.pose.theta);
 
     bool do_init = false;
     if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
@@ -133,8 +139,6 @@ namespace two_points_planner
 
     plan.clear();
     plan.push_back(start);
-    robot::log_info("[%s:%d]\n TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)", __FILE__, __LINE__,
-      start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
     
     unsigned int mx_start, my_start;
     unsigned int mx_end, my_end;

src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp

@@ -76,20 +76,21 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
 
     std::string algorithm_rotate_name = std::string("nav_algorithm::Rotate");
     planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
+    robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_rotate_name.c_str());
     try
     {
       robot::PluginLoaderHelper loader;
       std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
-      rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
+      nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
         path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
-      rotate_algorithm_ = rotate_algorithm_loader_();
+      nav_algorithm_ = nav_algorithm_loader_();
       if (!nav_algorithm_)
       {
         robot::log_error_at(__FILE__, __LINE__, "Failed to create algorithm \"%s\": returned null pointer", algorithm_rotate_name.c_str());
         exit(1);
       }
       robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_rotate_name);
-      rotate_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
+      nav_algorithm_->initialize(nh_algorithm, algorithm_rotate_name, tf, costmap_robot_, traj_generator_);
     }
     catch (const std::exception& ex)
     {
@@ -129,10 +130,13 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::reset()
 {
   robot::log_info_at(__FILE__, __LINE__, "New Goal Received.");
   this->unlock();
-  traj_generator_->reset();
+  if (traj_generator_)
-  goal_checker_->reset();
+    traj_generator_->reset();
-  nav_algorithm_->reset();
+  if (goal_checker_)
-  ret_nav_ = false;
+    goal_checker_->reset();
+  if (nav_algorithm_)
+    nav_algorithm_->reset();
+  ret_nav_ = ret_angle_ = false;
 }
 
 void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity)
@@ -150,12 +154,10 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
     if (!costmap_robot_->isCurrent())
       throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
   }
-
   // Update time stamp of goal pose
   // goal_pose_.header.stamp = pose.header.stamp;
   robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose),
     local_goal_pose = this->transformPoseToLocal(goal_pose_);
-
   try
   {
     if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))

src/Navigations/Packages/move_base/src/move_base.cpp

@@ -1139,9 +1139,25 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
 
   try
   {
+
+    robot_geometry_msgs::Pose2D pose;
+    if (!this->getRobotPose(pose))
+    {
+      if (nav_feedback_)
+      {
+        nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
+        nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
+        nav_feedback_->goal_checked = false;
+      }
+      return false;
+    }
+
     robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
     action_goal->header.stamp = robot::Time::now();
     action_goal->goal.target_pose = goal;
+    double distance = planner_costmap_robot_ ? planner_costmap_robot_->getCostmap()->getResolution() : 0.05;
+    action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
+    action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);
 
     // Generate unique goal ID using timestamp
     robot::Time now = robot::Time::now();