HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixbug

Browse Source
This commit is contained in:
HiepLM
2026-02-25 09:14:09 +07:00
parent 95c6fe0f1e
commit b7e4c73c14
3 changed files with 46 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
examples/NavigationExample/Program.cs
View File

@@ -579,8 +579,11 @@ namespace NavigationExample
NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped(); NavigationAPI.Twist2DStamped twist = new NavigationAPI.Twist2DStamped();
if (NavigationAPI.navigation_get_twist(navHandle, ref twist)) if (NavigationAPI.navigation_get_twist(navHandle, ref twist))
{ {
Console.WriteLine("Twist: {0}, {1}, {2}, {3}", NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta); Console.WriteLine(
} // Cleanup "Twist: {0}, {1}, {2}, {3}",
NavigationAPI.MarshalString(twist.header.frame_id), twist.velocity.x, twist.velocity.y, twist.velocity.theta);
}
// Cleanup
NavigationAPI.navigation_destroy(navHandle); NavigationAPI.navigation_destroy(navHandle);
NavigationAPI.tf_listener_destroy(tfHandle); NavigationAPI.tf_listener_destroy(tfHandle);
} }

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

66
src/APIs/c_api/src/nav_c_api.cpp
View File

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