update

src/APIs/c_api/include/nav_c_api.h

@@ -35,10 +35,12 @@ extern "C"
     void *ptr;
   } NavigationHandle;
   
-  typedef struct
+  // typedef struct
-  {
+  // {
-    void *ptr;
+  //   void *ptr;
-  } TFListenerHandle;
+  // } TFListenerHandle;
+
+  typedef void* TFListenerHandle;
   
   typedef enum
   {

src/APIs/c_api/src/nav_c_api.cpp

@@ -85,9 +85,9 @@ extern "C" TFListenerHandle tf_listener_create(void)
 
 extern "C" void tf_listener_destroy(TFListenerHandle handle)
 {
-  if (handle.ptr)
+  if (handle)
   {
-    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(handle.ptr);
+    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(handle);
     delete tf_ptr;
   }
 }
@@ -98,14 +98,14 @@ extern "C" bool tf_listener_set_static_transform(TFListenerHandle tf_handle,
                                                  double x, double y, double z,
                                                  double qx, double qy, double qz, double qw)
 {
-  if (!tf_handle.ptr || !parent_frame || !child_frame)
+  if (!tf_handle || !parent_frame || !child_frame)
   {
     return false;
   }
 
   try
   {
-    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(tf_handle.ptr);
+    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(tf_handle);
     if (!tf_ptr || !(*tf_ptr))
     {
       return false;
@@ -176,7 +176,7 @@ extern "C" void navigation_destroy(NavigationHandle handle)
 extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle)
 {
   printf("[%s:%d]\n Begin: navigation_initialize\n", __FILE__, __LINE__);
-  if (!handle.ptr || !tf_handle.ptr)
+  if (!handle.ptr || !tf_handle)
   {
     printf("[%s:%d]\n Error: Invalid parameters\n", __FILE__, __LINE__);
     return false;
@@ -188,7 +188,7 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
     if (!nav_ptr || !*nav_ptr)
       return false;
     auto *nav = nav_ptr->get();
-    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(tf_handle.ptr);
+    auto *tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore> *>(tf_handle);
     if (!tf_ptr || !(*tf_ptr))
     {
       printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);

src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp

@@ -85,6 +85,7 @@ void mkt_algorithm::diff::GoStraight::initialize(
 mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
     const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
 {
+  // robot::log_info("x %f y %f theta %f", pose.pose.x, pose.pose.y, pose.pose.theta);
   mkt_msgs::Trajectory2D result;
   result.velocity.x = result.velocity.y = result.velocity.theta = 0.0;
   if (!traj_)

src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp

@@ -49,7 +49,11 @@
 namespace robot_nav_core_adapter
 {
 
-  LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false)
+  LocalPlannerAdapter::LocalPlannerAdapter() 
+  : initialized_(false), 
+    tf_(nullptr), 
+    costmap_robot_(nullptr), 
+    has_active_goal_(false)
   {
   }
 
@@ -139,6 +143,15 @@ namespace robot_nav_core_adapter
     {
       return false;
     }
+    if(tf_ == nullptr)
+    {
+      robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
+      throw std::runtime_error("Failed to create the tf");
+    }
+    else
+    {
+      robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
+    }
     if (planner_name != last_config_.planner_name)
     {
       robot_nav_core2::LocalPlanner::Ptr old_planner = planner_;
@@ -157,15 +170,6 @@ namespace robot_nav_core_adapter
           robot::log_error_at(__FILE__, __LINE__, "Failed to load planner %s", planner_name.c_str());
           throw std::runtime_error("Failed to load planner " + planner_name);
         }
-        if(tf_ == nullptr)
-        {
-          robot::log_error("[%s:%d]\n ERROR: tf is nullptr", __FILE__, __LINE__);
-          throw std::runtime_error("Failed to create the tf");
-        }
-        else
-        {
-          robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
-        }
         new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
         planner_ = new_planner;
         planner_loader_ = new_loader;

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

@@ -341,6 +341,8 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
     }
     initialized_ = true;
     setup_ = true;
+    if(tf_)
+      robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
     robot::log_info("========== End: initialize() - SUCCESS ==========");
   }
   else
@@ -715,13 +717,38 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
   // robot::log_info("addOdometry: %s", odometry_name.c_str());
   // robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
   // robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
-  // robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x);
+  if(tf_)
-  // robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y);
+  {
-  // robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z);
+    // try
-  // robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x);
+    // {
-  // robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y);
+    //   robot_geometry_msgs::PoseStamped global_pose_stamped;
-  // robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z);
+    //   tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
-  // robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w);
+    //   robot_geometry_msgs::PoseStamped robot_pose;
+    //   tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
+    //   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))
+    //   {
+    //     // Transform is available at current time
+    //     tf3::TransformStampedMsg transform = tf_->lookupTransform("odom", "base_link", tf3_current_time);
+    //     tf3::doTransform(robot_pose, global_pose_stamped, transform);
+    //     robot::log_info("TF x: %f y: %f theta: %f", global_pose_stamped.pose.position.x, global_pose_stamped.pose.position.y, data_convert::getYaw(global_pose_stamped.pose.orientation));
+    //   }
+    // }
+    // catch (const tf3::TransformException &ex)
+    // {
+    //   robot::log_error("[addOdometry] Failed to lookup base_link in odom: %s", ex.what());
+    // }
+    robot::log_info("allFramesAsString: \n%s", tf_->allFramesAsString().c_str());
+  }
+  // robot::log_info("odometry x: %f y: %f theta: %f", odometry.pose.pose.position.x, odometry.pose.pose.position.y, data_convert::getYaw(odometry.pose.pose.orientation));
   // std::stringstream pose_covariance_str;
   // for(int i = 0; i < 36; i++) {
   //   pose_covariance_str << odometry.pose.covariance[i] << " ";