update

src/APIs/c_api/include/nav_c_api.h

@@ -141,6 +141,18 @@ extern "C"
    */
   bool navigation_move_to_order(NavigationHandle handle, const Order order, const PoseStamped goal);
 
+  /**
+   * @brief Send a goal for the robot to navigate to
+   * @param handle Navigation handle
+   * @param nodes Nodes array
+   * @param node_count Number of nodes in the array
+   * @param edges Edges array
+   * @param edge_count Number of edges in the array
+   * @param goal Target pose in the global frame
+   * @return true if goal was accepted and sent successfully
+   */
+  bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal);
+
   /**
    * @brief Send a docking goal to a predefined marker
    * @param handle Navigation handle

src/APIs/c_api/src/nav_c_api.cpp

@@ -171,6 +171,7 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
 
   try
   {
+
     std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
         std::shared_ptr<robot::move_base_core::BaseNavigation>(
             reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
@@ -181,10 +182,12 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
     footprint.reserve(point_count);
     for (size_t i = 0; i < point_count; ++i)
     {
+      
       robot_geometry_msgs::Point pt;
       pt.x = points[i].x;
       pt.y = points[i].y;
       pt.z = points[i].z;
+      printf("footprint x: %f, y: %f\n", pt.x, pt.y);
       footprint.push_back(pt);
     }
     nav_ptr->setRobotFootprint(footprint);
@@ -284,6 +287,40 @@ extern "C" bool navigation_move_to_order(NavigationHandle handle, const Order or
   }
 }
 
+extern "C" bool navigation_move_to_nodes_edges(NavigationHandle handle, const Node *nodes, size_t node_count, const Edge *edges, size_t edge_count, const PoseStamped goal)
+{
+  if (!handle)
+  {
+    return false;
+  }
+
+  try
+  {
+    std::shared_ptr<robot::move_base_core::BaseNavigation> nav_ptr =
+        std::shared_ptr<robot::move_base_core::BaseNavigation>(
+            reinterpret_cast<robot::move_base_core::BaseNavigation *>(handle), [](::robot::move_base_core::BaseNavigation *) {});
+    if (!nav_ptr)
+      return false;
+    robot::log_error("navigation_move_to_order goal %f %f", goal.pose.position.x, goal.pose.position.y);
+    Order order;
+    order.nodes = const_cast<Node *>(nodes);
+    order.nodes_count = node_count;
+    order.edges = const_cast<Edge *>(edges);
+    order.edges_count = edge_count;
+    robot_protocol_msgs::Order cpp_order = convert2CppOrder(order);
+    robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
+    return nav_ptr->moveTo(cpp_order, cpp_goal);
+  }
+  catch (const std::exception &e)
+  {
+    return false;
+  }
+  catch (...)
+  {
+    printf("[%s:%d]\n Error: Failed to move to order\n", __FILE__, __LINE__);
+    return false;
+  }
+}
 extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker, const PoseStamped goal)
 {
   if (!handle || !marker)

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

@@ -1,6 +1,7 @@
 #include <move_base/convert_data.h>
 #include <robot/robot.h>
 #include <robot_costmap_2d/cost_values.h>
+#include <fstream>
 
 char* move_base::ConvertData::cost_translation_table_ = NULL;
 
@@ -41,7 +42,19 @@ move_base::ConvertData::~ConvertData()
 // prepare grid_ message for publication.
 void move_base::ConvertData::prepareGrid()
 {
-  // boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_->getMutex()));
+  printf("Preparing costmap data for publication\n");
+
+  std::ofstream file("/home/robotics/sonvh/pnkx_nav_core/obstacle/hybrid_local_planner/cfg/costmap_data.txt");
+
+  if (!file.is_open())
+  {
+      std::cout << "Cannot open file!" << std::endl;
+      return;
+  }
+
+  // boost::unique_lock<costmap_2d::Costmap2D::mutex_t> 
+  // lock(*(costmap_->getCostmap()->getMutex()));
+
   double resolution = costmap_->getCostmap()->getResolution();
 
   grid_.header.frame_id = global_frame_;
@@ -53,22 +66,44 @@ void move_base::ConvertData::prepareGrid()
 
   double wx, wy;
   costmap_->getCostmap()->mapToWorld(0, 0, wx, wy);
+
   grid_.info.origin.position.x = wx - resolution / 2;
   grid_.info.origin.position.y = wy - resolution / 2;
   grid_.info.origin.position.z = 0.0;
   grid_.info.origin.orientation.w = 1.0;
+
   saved_origin_x_ = costmap_->getCostmap()->getOriginX();
   saved_origin_y_ = costmap_->getCostmap()->getOriginY();
 
   grid_.data.resize(grid_.info.width * grid_.info.height);
 
+  file << "frame: " << global_frame_ << std::endl;
+  file << "time: " << robot::Time::now().toSec() << std::endl;
+  file << "resolution: " << resolution << std::endl;
+  file << "width: " << grid_.info.width << std::endl;
+  file << "height: " << grid_.info.height << std::endl;
+  file << "origin_x: " << grid_.info.origin.position.x << std::endl;
+  file << "origin_y: " << grid_.info.origin.position.y << std::endl;
+
   unsigned char* data = costmap_->getCostmap()->getCharMap();
-  for (unsigned int i = 0; i < grid_.data.size(); i++)
+
+  for (unsigned int y = 0; y < grid_.info.height; y++)
   {
-    grid_.data[i] = cost_translation_table_[ data[ i ]];
+      for (unsigned int x = 0; x < grid_.info.width; x++)
+      {
+          unsigned int i = y * grid_.info.width + x;
+
+          grid_.data[i] = cost_translation_table_[data[i]];
+
+          file << int(grid_.data[i]) << " ";
+      }
+      file << "\n";
   }
+
+  file.close();
 }
 
+
 void move_base::ConvertData::updateCostmap(robot_nav_msgs::OccupancyGrid &grid, robot_map_msgs::OccupancyGridUpdate &update, bool &is_updated)
 {
   boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(costmap_->getCostmap()->getMutex()));

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

@@ -760,11 +760,11 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
   //     robot_pose.header.frame_id = robot_base_frame_;
   //     robot_pose.header.stamp = robot::Time();
   //     robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
-      
+        
   //     // Convert robot::Time to tf3::Time
   //     tf3::Time tf3_current_time = data_convert::convertTime(current_time);
   //     tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time());
-      
+        
   //     std::string error_msg;
   //     if (tf_->canTransform("odom", "base_link", tf3_current_time, &error_msg))
   //     {