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

update

Browse Source
This commit is contained in:
HiepLM 2026-01-16 15:13:14 +07:00
parent 0f58db3481
commit ebda1f81a1
33 changed files with 3597 additions and 515 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
config/costmap_common_params.yaml
View File

@ -1,3 +1,10 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
base_global_planner: CustomPlanner
robot_base_frame: base_footprint
transform_tolerance: 1.0
obstacle_range: 3.0

1
config/custom_global_params.yaml
View File

@ -1,4 +1,3 @@
base_global_planner: CustomPlanner
CustomPlanner:
library_path: libcustom_planner
environment_type: XYThetaLattice

24
config/pnkx_local_planner_params.yaml
View File

@ -1,10 +1,4 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: LocalPlannerAdapter
LocalPlannerAdapter:
library_path: liblocal_planner_adapter
yaw_goal_tolerance: 0.017
@ -92,7 +86,7 @@ MKTAlgorithmDiffPredictiveTrajectory:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 0.6 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 2.0 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.6 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
@ -120,6 +114,20 @@ MKTAlgorithmDiffPredictiveTrajectory:
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)
use_mpc: true
mpc_horizon: 10
mpc_dt: 0.1
mpc_w_pos: 6.0
mpc_w_theta: 2.0
mpc_w_v: 0.2
mpc_w_w: 0.2
mpc_w_dv: 0.4
mpc_w_dw: 0.4
mpc_iterations: 3
mpc_step: 0.15
mpc_eps: 0.001
MKTAlgorithmDiffGoStraight:
library_path: libmkt_algorithm_diff
avoid_obstacles: false
@ -133,7 +141,7 @@ MKTAlgorithmDiffGoStraight:
# only when false:
lookahead_dist: 0.5 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.8 # The minimum lookahead distance (m) threshold. (default: 0.3)
min_lookahead_dist: 1.0 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 2.5 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)

1
config/two_points_global_params.yaml
View File

@ -1,4 +1,3 @@
base_global_planner: TwoPointsPlanner
TwoPointsPlanner:
library_path: libtwo_points_planner
lethal_obstacle: 20

28
examples/CSharpExample.cs
View File

@ -53,6 +53,14 @@ namespace NavigationExample
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
@ -93,6 +101,13 @@ namespace NavigationExample
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2DStamped
{
public Header header;
public Twist2D velocity;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
@ -180,6 +195,14 @@ namespace NavigationExample
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_footprint(
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_points(IntPtr points);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
@ -231,6 +254,11 @@ namespace NavigationExample
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_twist(
IntPtr handle, ref Twist2DStamped out_twist);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(

28
examples/NavigationExample/Program.cs
View File

@ -53,6 +53,14 @@ namespace NavigationExample
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2D
{
public double x;
public double y;
public double theta;
}
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion
{
@ -93,6 +101,13 @@ namespace NavigationExample
public Pose pose;
}
[StructLayout(LayoutKind.Sequential)]
public struct Twist2DStamped
{
public Header header;
public Twist2D velocity;
}
[StructLayout(LayoutKind.Sequential)]
public struct Vector3
{
@ -180,6 +195,14 @@ namespace NavigationExample
public static extern bool navigation_set_robot_footprint(
IntPtr handle, Point[] points, UIntPtr point_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_robot_footprint(
IntPtr handle, out IntPtr out_points, out UIntPtr out_count);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
public static extern void navigation_free_points(IntPtr points);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_move_to(
@ -231,6 +254,11 @@ namespace NavigationExample
public static extern bool navigation_get_robot_pose_2d(
IntPtr handle, ref Pose2D out_pose);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_twist(
IntPtr handle, ref Twist2DStamped out_twist);
[DllImport(DllName, CallingConvention = CallingConvention.Cdecl)]
[return: MarshalAs(UnmanagedType.I1)]
public static extern bool navigation_get_feedback(

BIN
examples/NavigationExample/libnav_c_api.so
View File

Binary file not shown.

41
src/APIs/c_api/include/nav_c_api.h
View File

@ -59,6 +59,15 @@ typedef struct {
double theta;
} Pose2D;
/**
* @brief Twist2D structure (x, y, theta velocities)
*/
typedef struct {
double x;
double y;
double theta;
} Twist2D;
/**
* @brief Quaternion structure
*/
@ -104,6 +113,14 @@ typedef struct {
Pose pose;
} PoseStamped;
/**
* @brief Twist2DStamped structure
*/
typedef struct {
Header header;
Twist2D velocity;
} Twist2DStamped;
/**
* @brief Vector3 structure
*/
@ -250,6 +267,21 @@ bool navigation_initialize(NavigationHandle handle, TFListenerHandle tf_handle);
*/
bool navigation_set_robot_footprint(NavigationHandle handle, const Point* points, size_t point_count);
/**
* @brief Get the robot's footprint (outline shape)
* @param handle Navigation handle
* @param out_points Output array of points (allocated by library, free with navigation_free_points)
* @param out_count Output number of points in the array
* @return true on success, false on failure
*/
bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count);
/**
* @brief Free a points array allocated by navigation_get_robot_footprint
* @param points Pointer to point array
*/
void navigation_free_points(Point* points);
/**
* @brief Send a goal for the robot to navigate to
* @param handle Navigation handle
@ -350,6 +382,15 @@ bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseStamped* out
*/
bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* out_pose);
/**
* @brief Get the robot's current twist
* @param handle Navigation handle
* @param out_twist Output parameter with the robot's current twist
* @return true if twist was successfully retrieved
* @note out_twist->header.frame_id must be freed using nav_c_api_free_string
*/
bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist);
/**
* @brief Get navigation feedback
* @param handle Navigation handle

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

@ -6,12 +6,14 @@
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Vector3.h>
#include <robot_std_msgs/Header.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <tf3/buffer_core.h>
#include <string>
#include <vector>
#include <cstring>
#include <cstdlib>
#include <boost/dll/import.hpp>
#include <stdexcept>
#include <robot/robot.h>
@ -65,6 +67,25 @@ namespace {
c_pose->theta = cpp_pose.theta;
}
void cpp_to_c_header(const robot_std_msgs::Header& cpp_header, Header* c_header) {
c_header->seq = cpp_header.seq;
c_header->sec = cpp_header.stamp.sec;
c_header->nsec = cpp_header.stamp.nsec;
if (!cpp_header.frame_id.empty()) {
c_header->frame_id = strdup(cpp_header.frame_id.c_str());
} else {
c_header->frame_id = nullptr;
}
}
void cpp_to_c_twist2d_stamped(const robot_nav_2d_msgs::Twist2DStamped& cpp_twist,
Twist2DStamped* c_twist) {
cpp_to_c_header(cpp_twist.header, &c_twist->header);
c_twist->velocity.x = cpp_twist.velocity.x;
c_twist->velocity.y = cpp_twist.velocity.y;
c_twist->velocity.theta = cpp_twist.velocity.theta;
}
// Convert C++ NavFeedback to C NavFeedback
void cpp_to_c_nav_feedback(const robot::move_base_core::NavFeedback& cpp_feedback, NavFeedback* c_feedback) {
c_feedback->navigation_state = static_cast<NavigationState>(static_cast<int>(cpp_feedback.navigation_state));
@ -226,22 +247,13 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle
}
try {
printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__);
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
auto* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
auto* tf_ptr = static_cast<std::shared_ptr<tf3::BufferCore>*>(tf_handle);
robot::PluginLoaderHelper loader;
std::string lib_path = loader.findLibraryPath("MoveBase");
auto create_plugin = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
lib_path,
"MoveBase",
boost::dll::load_mode::append_decorations);
robot::move_base_core::BaseNavigation::Ptr nav_ptr = create_plugin();
printf("[%s:%d]\n Navigation created\n", __FILE__, __LINE__);
if (nav_ptr == nullptr) {
printf("[%s:%d]\n Error: Failed to create navigation\n", __FILE__, __LINE__);
if (!tf_ptr || !(*tf_ptr)) {
printf("[%s:%d]\n Error: Invalid TF listener\n", __FILE__, __LINE__);
return false;
}
nav_ptr->initialize(*tf_ptr);
nav->initialize(*tf_ptr);
return true;
} catch (const std::exception &e) {
@ -276,6 +288,45 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
}
}
extern "C" bool navigation_get_robot_footprint(NavigationHandle handle, Point** out_points, size_t* out_count) {
if (!handle || !out_points || !out_count) {
return false;
}
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
std::vector<robot_geometry_msgs::Point> footprint = nav->getRobotFootprint();
if (footprint.empty()) {
*out_points = nullptr;
*out_count = 0;
return true;
}
Point* points = static_cast<Point*>(malloc(sizeof(Point) * footprint.size()));
if (!points) {
return false;
}
for (size_t i = 0; i < footprint.size(); ++i) {
points[i].x = footprint[i].x;
points[i].y = footprint[i].y;
points[i].z = footprint[i].z;
}
*out_points = points;
*out_count = footprint.size();
return true;
} catch (...) {
return false;
}
}
extern "C" void navigation_free_points(Point* points) {
if (points) {
free(points);
}
}
extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* goal,
double xy_goal_tolerance, double yaw_goal_tolerance) {
if (!handle || !goal) {
@ -442,6 +493,21 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
}
}
extern "C" bool navigation_get_twist(NavigationHandle handle, Twist2DStamped* out_twist) {
if (!handle || !out_twist) {
return false;
}
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
robot_nav_2d_msgs::Twist2DStamped cpp_twist = nav->getTwist();
cpp_to_c_twist2d_stamped(cpp_twist, out_twist);
return true;
} catch (...) {
return false;
}
}
extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* out_feedback) {
if (!handle || !out_feedback) {
return false;

1
src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
View File

@ -84,6 +84,7 @@ add_library(${PROJECT_NAME}_diff SHARED
src/diff/diff_predictive_trajectory.cpp
src/diff/diff_rotate_to_goal.cpp
src/diff/diff_go_straight.cpp
# src/diff/pure_pursuit.cpp
)
if(BUILDING_WITH_CATKIN)

50
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/pure_pursuit.h Normal file
View File

@ -0,0 +1,50 @@
#ifndef _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PURE_PURSUIT_H_INCLUDED__
#include <robot/robot.h>
#include <score_algorithm/score_algorithm.h>
#include <boost/dll/alias.hpp>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
namespace mkt_algorithm
{
namespace diff
{
class PurePursuit : public score_algorithm::ScoreAlgorithm
{
public:
PurePursuit();
~PurePursuit();
void computePurePursuit(
const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override);
private:
void applyConstraints(const double &dist_error, const double &lookahead_dist,
const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity,
const double &pose_cost, double &linear_vel, double &sign_x);
double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &journey_plan);
double computeDecelerationFactor(const double &effective_journey, const double &d_reduce);
bool detectWobbleByCarrotAngle(std::vector<double> &angle_history, const double &carrot_angle,
const double &amplitude_threshold, const size_t &window_size);
};
}
}
#endif

3
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp
View File

@ -107,7 +107,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
robot_nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
robot::Rate r(50);
while (traj_->hasMoreTwists())
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
// ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@ -184,6 +184,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
journey_plan += max_path_distance_;

1327
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy 2.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1320
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory copy.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

12
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@ -391,15 +391,25 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
robot_nav_2d_msgs::Twist2D drive_cmd;
double sign_x = sign(x_direction_);
robot_nav_2d_msgs::Twist2D twist;
robot::log_info("[%s:%d] velocity in predictive trajectory: %f %f %f", __FILE__, __LINE__, velocity.x, velocity.y, velocity.theta);
traj_->startNewIteration(velocity);
while (traj_->hasMoreTwists())
while (robot::ok() && traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
}
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
robot::log_info("[%s:%d] twist: %f %f %f", __FILE__, __LINE__, twist.x, twist.y, twist.theta);
robot::log_info("--------------------------------");
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
result.poses.clear();
result.poses.reserve(transformed_plan.poses.size());
for (const auto &pose_stamped : transformed_plan.poses)
{
result.poses.push_back(pose_stamped.pose);
}
if (transformed_plan.poses.empty())
{
robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);

52
src/Algorithms/Libraries/mkt_algorithm/src/diff/pure_pursuit.cpp Normal file
View File

@ -0,0 +1,52 @@
#include <mkt_algorithm/diff/pure_pursuit.h>
void mkt_algorithm::diff::PurePursuit::computePurePursuit(const double &velocity_min,
const double &velocity_max,
const double &linear_vel,
const robot_nav_2d_msgs::Pose2DStamped &carrot_pose,
const double &min_dist2,
const double &curvature_dist2_floor,
const double &dist2_override)
{
const double vel_x_reduce = std::min(fabs(velocity_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
if (max_lateral_accel_ > 1e-6)
{
const double curvature_abs = std::max(fabs(curvature), 1e-6);
const double v_lateral_limit = sqrt(max_lateral_accel_ / curvature_abs);
drive_cmd.x = sign_x > 0 ? std::min(drive_cmd.x, v_lateral_limit) : std::max(drive_cmd.x, -v_lateral_limit);
}
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
}

17
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h
View File

@ -270,6 +270,23 @@ namespace mkt_plugins
using Ptr = std::shared_ptr<KinematicParameters>;
protected:
/**
* @brief Configure the kinematic parameters
* @param nh NodeHandle
*
* Configures the kinematic parameters from the given NodeHandle.
*/
void configure(const robot::NodeHandle &nh);
/**
* @brief Reconfigure the kinematic parameters
* @param nh NodeHandle
*
* Reconfigures the kinematic parameters from the given NodeHandle.
*/
void reconfigure(const robot::NodeHandle &nh);
// For parameter descriptions, see cfg/KinematicParams.cfg
int xytheta_direct_[3];
double min_vel_x_, min_vel_y_;

10
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h
View File

@ -116,9 +116,13 @@ namespace mkt_plugins
* current velocity within acc_time seconds, respecting acceleration/deceleration limits.
* The velocities range from min_vel_ to max_vel_ (projected from current velocity).
*/
OneDVelocityIterator(double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time,
int num_samples)
OneDVelocityIterator(
std::string name, double current, int direct, double min, double max, double acc_limit, double decel_limit, double acc_time, int num_samples)
: name_(name)
{
if(name_ == "x_it_")
robot::log_warning("[%s:%d] one_d_velocity_iterator: [%s] %f %d %f %f %f %f %f %d",
__FILE__, __LINE__, name_.c_str(), current, direct, min, max, acc_limit, decel_limit, acc_time, num_samples);
// if (current < min)
// {
// current = min;
@ -203,8 +207,6 @@ namespace mkt_plugins
*/
bool isFinished() const
{
// if(name_ == std::string("th_it_"))
// ROS_INFO("%s %f %f", name_.c_str(), current_, max_vel_ + EPSILON);
double limit_vel = direct_ > 0? max_vel_ : min_vel_;
return fabs(current_) > fabs(limit_vel) + EPSILON;
}

42
src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp
View File

@ -54,10 +54,48 @@ namespace mkt_plugins
setDecelerationAsNeeded(nh, "y");
setDecelerationAsNeeded(nh, "theta");
configure(nh);
xytheta_direct_[0] = xytheta_direct_[1] = xytheta_direct_[2] = 1;
}
void KinematicParameters::configure(const robot::NodeHandle &nh)
{
nh.param("max_vel_x", original_max_vel_x_, 0.0);
nh.param("max_vel_y", original_max_vel_y_, 0.0);
nh.param("max_vel_theta", original_max_vel_theta_, 0.0);
nh.param("min_vel_x", original_min_vel_x_, 0.0);
nh.param("min_vel_y", original_min_vel_y_, 0.0);
nh.param("min_speed_xy", original_min_speed_xy_, 0.0);
nh.param("max_speed_xy", original_max_speed_xy_, 0.0);
nh.param("min_speed_theta", original_min_speed_theta_, 0.0);
nh.param("acc_lim_x", original_acc_lim_x_, 0.0);
nh.param("acc_lim_y", original_acc_lim_y_, 0.0);
nh.param("acc_lim_theta", original_acc_lim_theta_, 0.0);
nh.param("decel_lim_x", original_decel_lim_x_, 0.0);
nh.param("decel_lim_y", original_decel_lim_y_, 0.0);
nh.param("decel_lim_theta", original_decel_lim_theta_, 0.0);
reconfigure(nh);
}
void KinematicParameters::reconfigure(const robot::NodeHandle &nh)
{
nh.param("max_vel_x", max_vel_x_, 0.0);
nh.param("max_vel_y", max_vel_y_, 0.0);
nh.param("max_vel_theta", max_vel_theta_, 0.0);
nh.param("min_vel_x", min_vel_x_, 0.0);
nh.param("min_vel_y", min_vel_y_, 0.0);
nh.param("min_speed_xy", min_speed_xy_, 0.0);
nh.param("max_speed_xy", max_speed_xy_, 0.0);
nh.param("min_speed_theta", min_speed_theta_, 0.0);
nh.param("acc_lim_x", acc_lim_x_, 0.0);
nh.param("acc_lim_y", acc_lim_y_, 0.0);
nh.param("acc_lim_theta", acc_lim_theta_, 0.0);
nh.param("decel_lim_x", decel_lim_x_, 0.0);
nh.param("decel_lim_y", decel_lim_y_, 0.0);
nh.param("decel_lim_theta", decel_lim_theta_, 0.0);
}
void KinematicParameters::setDirect(int *xytheta_direct)
{
xytheta_direct_[0] = xytheta_direct[0];
@ -111,13 +149,11 @@ namespace mkt_plugins
void KinematicParameters::setMinX(double value)
{
min_vel_x_ = fabs(value) < fabs(original_min_vel_x_) + EPSILON ? value : min_vel_x_;
// ROS_INFO_THROTTLE(10.0, "vx_min %f %f %f", value , original_min_vel_x_, min_vel_x_);
}
void KinematicParameters::setMaxX(double value)
{
max_vel_x_ = fabs(value) <= fabs(original_max_vel_x_) + EPSILON ? value : original_max_vel_x_;
// ROS_INFO_THROTTLE(10, "vx %f %f %f", value , original_max_vel_x_, max_vel_x_);
}
void KinematicParameters::setAccX(double value)

2
src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp
View File

@ -18,6 +18,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
else
{
double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0);
robot::log_warning("[%s:%d] limited_accel_generator: %f", __FILE__, __LINE__, controller_frequency);
if (controller_frequency > 0)
{
acceleration_time_ = 1.0 / controller_frequency;
@ -29,6 +30,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh)
acceleration_time_ = 0.05;
}
}
}
void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity)

12
src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp
View File

@ -14,19 +14,15 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter
void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt)
{
x_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
robot::log_info("[%s:%d] xy_theta_iterator: %f %f %f %f", __FILE__, __LINE__, current_velocity.x, current_velocity.y, current_velocity.theta, dt);
x_it_ = std::make_shared<OneDVelocityIterator>("x_it_", current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(),
kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_);
x_it_->name_ = std::string("x_it_");
y_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
y_it_ = std::make_shared<OneDVelocityIterator>("y_it_", current_velocity.y, kinematics_->getDirect()[1], kinematics_->getMinY(), kinematics_->getMaxY(),
kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_);
y_it_->name_ = std::string("y_it_");
th_it_ = std::make_shared<OneDVelocityIterator>(current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
th_it_ = std::make_shared<OneDVelocityIterator>("th_it_", current_velocity.theta, kinematics_->getDirect()[2], kinematics_->getMinTheta(), kinematics_->getMaxTheta(),
kinematics_->getAccTheta(), kinematics_->getDecelTheta(), dt, vtheta_samples_);
th_it_->name_ = std::string("th_it_");
}
bool XYThetaIterator::hasMoreTwists()

2
src/Algorithms/Packages/global_planners/custom_planner

@ -1 +1 @@
Subproject commit a2bebb5be969471a135601da78866cc9d1db84b1
Subproject commit 49afcce5b2247793282fe5e94d0e27f062562325

9
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
View File

@ -46,6 +46,12 @@ namespace pnkx_local_planner
*/
void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief robot_nav_core2 getPlan - Gets the current global plan
* @param path The global plan
*/
void getPlan(robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
@ -179,7 +185,8 @@ namespace pnkx_local_planner
std::string name_;
robot::NodeHandle parent_, planner_nh_;
robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
robot_nav_2d_msgs::Path2D transformed_plan_;
robot_nav_2d_msgs::Path2D transformed_global_plan_;
robot_nav_2d_msgs::Path2D local_plan_;
robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
robot_costmap_2d::Costmap2D *costmap_;

17
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@ -180,7 +180,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const robot_nav_core2::LocalPlannerException& e)
@ -189,7 +189,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
@ -206,9 +206,16 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
robot_nav_2d_msgs::Twist2D twist;
mkt_msgs::Trajectory2D traj;
robot_nav_2d_msgs::Twist2DStamped cmd_vel;
if (!ret_nav_)
traj = nav_algorithm_->calculator(pose, velocity);
if (ret_nav_)
{
local_plan_.poses.clear();
return cmd_vel;
}
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;
}
@ -222,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
}
// Update time stamp of goal pose
// goal_pose_.header.stamp = pose.header.stamp;
ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_plan_, velocity);
ret_nav_ = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), transformed_global_plan_, velocity);
if (ret_nav_)
robot::log_info_at(__FILE__, __LINE__, "Goal reached!");

31
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@ -208,6 +208,15 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path
}
}
void pnkx_local_planner::PNKXLocalPlanner::getPlan(robot_nav_2d_msgs::Path2D &path)
{
if (local_plan_.poses.empty())
{
return;
}
path = local_plan_;
}
void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity)
{
this->getParams(planner_nh_);
@ -231,7 +240,8 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
local_goal_pose = this->transformPoseToLocal(goal_pose_);
// robot::log_info("local_start_pose: %f %f %f", local_start_pose.pose.x, local_start_pose.pose.y, local_start_pose.pose.theta);
// robot::log_info("local_goal_pose: %f %f %f", local_goal_pose.pose.x, local_goal_pose.pose.y, local_goal_pose.pose.theta);
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(
tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
{
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
@ -240,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
double x_direction, y_direction, theta_direction;
if (!ret_nav_)
{
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -248,7 +258,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
}
else
{
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
@ -291,11 +301,24 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
cmd_vel.header.stamp = robot::Time::now();
if (ret_nav_ && ret_angle_)
{
return cmd_vel;
local_plan_.poses.clear();
}
else if (!ret_nav_)
{
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
}
else
{
traj = rotate_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
}
cmd_vel.velocity = traj.velocity;
return cmd_vel;
@ -404,7 +427,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs
// goal_pose_.header.stamp = pose.header.stamp;
robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose);
robot_nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_);
robot_nav_2d_msgs::Path2D plan = transformed_plan_;
robot_nav_2d_msgs::Path2D plan = transformed_global_plan_;
if (!ret_nav_)
{
ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity);

11
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
View File

@ -155,7 +155,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_global_plan_))
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const robot_nav_core2::LocalPlannerException& e)
@ -164,7 +164,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_global_plan_, x_direction, y_direction, theta_direction))
{
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
@ -199,10 +199,17 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::Sc
robot_nav_2d_msgs::Twist2D twist;
mkt_msgs::Trajectory2D traj;
robot_nav_2d_msgs::Twist2DStamped cmd_vel;
cmd_vel.header.stamp = robot::Time::now();
if (ret_nav_)
{
local_plan_.poses.clear();
return cmd_vel;
}
traj = nav_algorithm_->calculator(pose, velocity);
local_plan_.header.stamp = robot::Time::now();
robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(traj.poses, costmap_robot_->getGlobalFrameID(), robot::Time::now());
local_plan_ = robot_nav_2d_utils::pathToPath(path);
cmd_vel.velocity = traj.velocity;
return cmd_vel;
}

16
src/Libraries/tf3/include/tf3/LinearMath/QuadWord.h
View File

@ -159,20 +159,20 @@ protected:
*/
TF3SIMD_FORCE_INLINE void setMax(const QuadWord& other)
{
// tf3SetMax(m_floats[0], other.m_floats[0]);
// tf3SetMax(m_floats[1], other.m_floats[1]);
// tf3SetMax(m_floats[2], other.m_floats[2]);
// tf3SetMax(m_floats[3], other.m_floats[3]);
tf3SetMax(m_floats[0], other.m_floats[0]);
tf3SetMax(m_floats[1], other.m_floats[1]);
tf3SetMax(m_floats[2], other.m_floats[2]);
tf3SetMax(m_floats[3], other.m_floats[3]);
}
/**@brief Set each element to the min of the current values and the values of another QuadWord
* @param other The other QuadWord to compare with
*/
TF3SIMD_FORCE_INLINE void setMin(const QuadWord& other)
{
// tf3SetMin(m_floats[0], other.m_floats[0]);
// tf3SetMin(m_floats[1], other.m_floats[1]);
// tf3SetMin(m_floats[2], other.m_floats[2]);
// tf3SetMin(m_floats[3], other.m_floats[3]);
tf3SetMin(m_floats[0], other.m_floats[0]);
tf3SetMin(m_floats[1], other.m_floats[1]);
tf3SetMin(m_floats[2], other.m_floats[2]);
tf3SetMin(m_floats[3], other.m_floats[3]);
}

7
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@ -27,11 +27,10 @@
#include <robot/time.h>
#include <move_base_core/common.h>
namespace robot
{
namespace move_base_core
{
namespace move_base_core
{
// Navigation states, including planning and controller status
enum class State
{
@ -515,7 +514,7 @@ namespace move_base_core
BaseNavigation() = default;
};
} // namespace move_base_core
} // namespace move_base_core
};
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

7
src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@ -133,6 +133,13 @@ namespace robot_nav_core
*/
virtual bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) = 0;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) = 0;
/**
* @brief Constructs the local planner
* @param name The name to give this instance of the local planner

7
src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@ -92,6 +92,13 @@ namespace robot_nav_core2
*/
virtual void setPlan(const robot_nav_2d_msgs::Path2D &path) = 0;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(robot_nav_2d_msgs::Path2D &path) = 0;
/**
* @brief Compute the best command given the current pose, velocity and goal
*

8
src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@ -159,6 +159,14 @@ namespace robot_nav_core_adapter
*/
bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped> &plan) override;
/**
* @brief Gets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path) override;
/**
* @brief Create a new LocalPlannerAdapter
* @return A shared pointer to the new LocalPlannerAdapter

11
src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@ -366,6 +366,17 @@ namespace robot_nav_core_adapter
}
}
void LocalPlannerAdapter::getPlan(std::vector<robot_geometry_msgs::PoseStamped> &path)
{
if (!planner_)
{
return;
}
robot_nav_2d_msgs::Path2D path2d;
planner_->getPlan(path2d);
path = robot_nav_2d_utils::pathToPath(path2d).poses;
}
bool LocalPlannerAdapter::hasGoalChanged(const robot_nav_2d_msgs::Pose2DStamped &new_goal)
{
if (last_goal_.header.frame_id != new_goal.header.frame_id ||

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

@ -607,7 +607,6 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L
void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
robot::log_info("[%s:%d] addOdometry called for: %s", __FILE__, __LINE__, odometry_name.c_str());
odometry_ = odometry;
}
@ -2206,6 +2205,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
{
if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel))
{
robot_nav_msgs::Path path;
tc_->getPlan(path.poses);
if (!path.poses.empty())
{
path.header.stamp = path.poses[0].header.stamp;
path.header.frame_id = path.poses[0].header.frame_id;
}
else
{
path.header.stamp = robot::Time::now();
path.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
}
local_data_.plan = robot_nav_2d_utils::pathToPath(path);
last_valid_control_ = robot::Time::now();
// make sure that we send the velocity command to the base
@ -2283,6 +2296,9 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal)
lock.unlock();
}
}
twist_.velocity = robot_nav_2d_utils::twist3Dto2D(cmd_vel);
twist_.header.stamp = robot::Time::now();
twist_.header.frame_id = controller_costmap_robot_->getGlobalFrameID();
}
else
{