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

fix speed

Browse Source
This commit is contained in:
QUYVN
2026-03-24 08:16:56 +00:00
parent 5375a5ea84
commit 6b4d630d09
2 changed files with 20 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -572,6 +572,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
linear.x = linear_x; linear.x = linear_x;
linear.y = linear_y; linear.y = linear_y;
linear.z = linear_z; linear.z = linear_z;
robot::log_info("setTwistLinear %f", linear.x);
return nav_ptr->setTwistLinear(linear); return nav_ptr->setTwistLinear(linear);
} }
catch (...) catch (...)

20
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -976,6 +976,12 @@ bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
as_->processGoal(action_goal); as_->processGoal(action_goal);
setTwistLinear(old_linear_fwd_);
setTwistLinear(old_linear_bwd_);
setTwistAngular(old_angular_fwd_);
setTwistAngular(old_angular_rev_);
robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
} }
catch (const std::exception &e) catch (const std::exception &e)
@@ -1118,6 +1124,11 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
} }
as_->processGoal(action_goal); as_->processGoal(action_goal);
setTwistLinear(old_linear_fwd_);
setTwistLinear(old_linear_bwd_);
setTwistAngular(old_angular_fwd_);
setTwistAngular(old_angular_rev_);
} }
catch (const std::exception &e) catch (const std::exception &e)
{ {
@@ -1279,6 +1290,10 @@ bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry
robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); robot::log_info("[MoveBase::moveTo] Processing goal through action server...");
as_->processGoal(action_goal); as_->processGoal(action_goal);
setTwistLinear(old_linear_fwd_);
setTwistLinear(old_linear_bwd_);
setTwistAngular(old_angular_fwd_);
setTwistAngular(old_angular_rev_);
robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server");
} }
catch (const std::exception &e) catch (const std::exception &e)
@@ -1424,8 +1439,11 @@ bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
lock.unlock(); lock.unlock();
return false; return false;
} }
as_->processGoal(action_goal); as_->processGoal(action_goal);
setTwistLinear(old_linear_fwd_);
setTwistLinear(old_linear_bwd_);
setTwistAngular(old_angular_fwd_);
setTwistAngular(old_angular_rev_);
} }
catch (const std::exception &e) catch (const std::exception &e)
{ {