fixbug

src/APIs/c_api/src/nav_c_api.cpp

@@ -286,8 +286,10 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped go
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
     return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
   }
@@ -324,8 +326,10 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char *marker,
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
     return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
   }
@@ -360,8 +364,10 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
     return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
   }
@@ -380,8 +386,10 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::PoseStamped cpp_goal = convert2CppPoseStamped(goal);
     return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
   }
@@ -397,9 +405,9 @@ extern "C" void navigation_pause(NavigationHandle handle)
   {
     try
     {
-      robot::move_base_core::BaseNavigation *nav =
-          static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
-      nav->pause();
+      auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+      if (nav_ptr && *nav_ptr)
+        nav_ptr->get()->pause();
     }
     catch (...)
     {
@@ -414,9 +422,9 @@ extern "C" void navigation_resume(NavigationHandle handle)
   {
     try
     {
-      robot::move_base_core::BaseNavigation *nav =
-          static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
-      nav->resume();
+      auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+      if (nav_ptr && *nav_ptr)
+        nav_ptr->get()->resume();
     }
     catch (...)
     {
@@ -431,9 +439,9 @@ extern "C" void navigation_cancel(NavigationHandle handle)
   {
     try
     {
-      robot::move_base_core::BaseNavigation *nav =
-          static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
-      nav->cancel();
+      auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+      if (nav_ptr && *nav_ptr)
+        nav_ptr->get()->cancel();
     }
     catch (...)
     {
@@ -452,8 +460,10 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::Vector3 linear;
     linear.x = linear_x;
     linear.y = linear_y;
@@ -476,8 +486,10 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::Vector3 angular;
     angular.x = angular_x;
     angular.y = angular_y;
@@ -499,8 +511,10 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::PoseStamped cpp_pose;
     if (nav->getRobotPose(cpp_pose))
     {
@@ -524,8 +538,10 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D &ou
 
   try
   {
-    robot::move_base_core::BaseNavigation *nav =
-        static_cast<robot::move_base_core::BaseNavigation *>(handle.ptr);
+    auto *nav_ptr = static_cast<std::shared_ptr<robot::move_base_core::BaseNavigation> *>(handle.ptr);
+    if (!nav_ptr || !*nav_ptr)
+      return false;
+    auto *nav = nav_ptr->get();
     robot_geometry_msgs::Pose2D cpp_pose;
     if (nav->getRobotPose(cpp_pose))
     {