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

update

Browse Source
This commit is contained in:
HiepLM
2026-03-02 07:50:30 +00:00
parent ff8a90cbaa
commit 06c2d01b4a
61 changed files with 2934 additions and 1012 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -406,7 +406,8 @@ namespace robot
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;

1
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -303,6 +303,7 @@ namespace move_base
* @return True if docking command succeeded.
*/
virtual bool dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) override;

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

@@ -33,20 +33,50 @@
#include <robot_nav_2d_utils/conversions.h>
move_base::MoveBase::MoveBase()
: initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false)
: initialized_(false),
tf_(),
as_(NULL),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
recovery_index_(0),
recovery_behavior_enabled_(false),
planner_frequency_(0.0), controller_frequency_(0.0),
inscribed_radius_(0.0), circumscribed_radius_(0.0),
planner_patience_(0.0), controller_patience_(0.0),
max_planning_retries_(0), planning_retries_(0),
conservative_reset_dist_(0.0), clearing_radius_(0.0),
shutdown_costmaps_(false), clearing_rotation_allowed_(false),
make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
oscillation_timeout_(0.0), oscillation_distance_(0.0),
state_(PLANNING), recovery_trigger_(PLANNING_R),
runPlanner_(false), planner_thread_(NULL),
setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false), cancel_ctr_(false),
original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
}
move_base::MoveBase::MoveBase(robot::TFListenerPtr tf)
: initialized_(false),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false)
: initialized_(false),
tf_(),
as_(NULL),
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
recovery_index_(0),
recovery_behavior_enabled_(false),
planner_frequency_(0.0), controller_frequency_(0.0),
inscribed_radius_(0.0), circumscribed_radius_(0.0),
planner_patience_(0.0), controller_patience_(0.0),
max_planning_retries_(0), planning_retries_(0),
conservative_reset_dist_(0.0), clearing_radius_(0.0),
shutdown_costmaps_(false), clearing_rotation_allowed_(false),
make_plan_clear_costmap_(false), make_plan_add_unreachable_goal_(false),
oscillation_timeout_(0.0), oscillation_distance_(0.0),
state_(PLANNING), recovery_trigger_(PLANNING_R),
runPlanner_(false), planner_thread_(NULL),
setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false),
pause_ctr_(false), paused_(false), cancel_ctr_(false),
original_xy_goal_tolerance_(0.0), original_yaw_goal_tolerance_(0.0)
{
initialize(tf);
}
@@ -134,6 +164,11 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::log_info("[%s:%d]\n INFO: tf is not nullptr", __FILE__, __LINE__);
}
tf_ = tf;
if(tf_)
{
std::string all_frames_string = tf_->allFramesAsString();
robot::log_info("[%s:%d]\n INFO: tf_ allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
}
as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true);
setupActionServerCallbacks();
@@ -341,8 +376,6 @@ 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
@@ -717,37 +750,37 @@ 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());
if(tf_)
{
// try
// {
// robot_geometry_msgs::PoseStamped global_pose_stamped;
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
// 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
// if(tf_)
// {
// try
// {
// robot_geometry_msgs::PoseStamped global_pose_stamped;
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
// 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());
// // 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());
}
// 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++) {
@@ -770,7 +803,8 @@ void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_na
odometry_ = odometry;
}
bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance)
bool move_base::MoveBase::moveTo( const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
if (setup_)
{
@@ -816,12 +850,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
// Check pointers
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
@@ -829,7 +876,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
// Swap planner
try
{
if(!tc_->swapPlanner(position_planner_name_))
if (!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
@@ -837,8 +884,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
}
catch (const std::exception &e)
{
std::cerr << e.what() << "\n";
throw std::exception(e);
robot::log_error("[MoveBase::moveTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
// Update navigation feedback
@@ -935,20 +983,33 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
robot::Duration(0.01).sleep();
robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if(!tc_->swapPlanner(position_planner_name_))
if (!tc_->swapPlanner(position_planner_name_))
{
robot::log_error("[MoveBase::moveTo] Failed to swapPlanner");
return false;
@@ -967,49 +1028,76 @@ bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg,
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread
if (!as_)
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::moveTo] Stored Order message for planning");
}
if (!planner_order_)
{
robot::log_error("[MoveBase::moveTo] Failed to store Order message for planning");
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what());
lock.unlock();
return false;
}
as_->processGoal(action_goal);
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal,
bool move_base::MoveBase::dockTo(const std::string &marker, const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
std::string maker_sources;
// private_nh_.param("maker_sources", maker_sources, std::string(""));
private_nh_.param("maker_sources", maker_sources, std::string(""));
std::stringstream ss(maker_sources);
std::string source;
bool has_maker = false;
while (ss >> source)
{
if (maker == source)
if (marker == source)
{
private_nh_.setParam("maker_name", maker);
private_nh_.setParam("maker_name", marker);
has_maker = true;
}
}
@@ -1019,7 +1107,120 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
std::stringstream fb_ss;
fb_ss << "The system has not been '" << maker << "'";
fb_ss << "The system has not been '" << marker << "'";
nav_feedback_->feed_back_str = fb_ss.str();
nav_feedback_->goal_checked = false;
}
return false;
}
if (setup_)
{
swapPlanner(default_config_.base_global_planner);
}
else
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("The system has not been installed yet");
nav_feedback_->goal_checked = false;
}
return false;
}
if (fabs(xy_goal_tolerance) > 0.001)
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (fabs(yaw_goal_tolerance) > 0.001)
this->setYawGoalTolerance(fabs(yaw_goal_tolerance));
else
this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_));
if (!tc_)
{
robot::log_error("[MoveBase::dockTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
robot::log_error("[MoveBase::dockTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if (!tc_->swapPlanner(docking_planner_name_))
{
robot::log_error("[MoveBase::dockTo] Failed to swapPlanner");
return false;
}
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::dockTo] Exception in swapPlanner: %s", e.what());
lock.unlock();
return false;
}
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING;
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::dockTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const std::string &marker,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
std::string maker_sources;
private_nh_.param("maker_sources", maker_sources, std::string(""));
std::stringstream ss(maker_sources);
std::string source;
bool has_maker = false;
while (ss >> source)
{
if (marker == source)
{
private_nh_.setParam("maker_name", marker);
has_maker = true;
}
}
if (!has_maker)
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
std::stringstream fb_ss;
fb_ss << "The system has not been '" << marker << "'";
nav_feedback_->feed_back_str = fb_ss.str();
nav_feedback_->goal_checked = false;
}
@@ -1084,17 +1285,53 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
if (cancel_ctr_)
cancel_ctr_ = false;
if (!as_)
{
robot::log_error("[MoveBase::dockTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_dock_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::dockTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
// Store Order message for use in planThread (same as moveTo with Order)
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_ = boost::make_shared<robot_protocol_msgs::Order>(msg);
robot::log_info("[MoveBase::dockTo] Stored Order message for planning");
}
if (!planner_order_)
{
robot::log_error("[MoveBase::dockTo] Failed to store Order message for planning");
lock.unlock();
return false;
}
as_->processGoal(action_goal);
}
catch (const std::exception &e)
{
robot::log_error("[MoveBase::dockTo] Exception during processGoal: %s", e.what());
lock.unlock();
return false;
}
lock.unlock();
return true;
}
bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg,
const robot_geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance, double yaw_goal_tolerance)
{
return false;
}
bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance)
{
robot::log_info("[MoveBase::moveStraightTo] Entry");
@@ -1117,13 +1354,28 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
this->setXyGoalTolerance(fabs(xy_goal_tolerance));
else
this->setXyGoalTolerance(fabs(original_xy_goal_tolerance_));
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::moveStraightTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
@@ -1131,17 +1383,10 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
try
{
robot::log_info("[MoveBase::moveStraightTo] Entry swapPlanner");
if(!tc_->swapPlanner(go_straight_planner_name_))
if (!tc_->swapPlanner(go_straight_planner_name_))
{
if(tf_ == nullptr)
{
robot::log_error("[MoveBase::moveTo] tf_ pointer is null!");
throw std::runtime_error("Null 'tf_' pointer encountered");
}
else
{
robot::log_info("[MoveBase::moveTo] tf_ pointer is not null!");
}
robot::log_error("[MoveBase::moveStraightTo] Failed to swapPlanner");
lock.unlock();
return false;
}
}
@@ -1158,13 +1403,18 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::moveStraightTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
robot::log_error("[MoveBase::moveStraightTo] as_ pointer is null!");
lock.unlock();
return false;
}
@@ -1174,31 +1424,24 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared<robot_move_base_msgs::MoveBaseActionGoal>();
action_goal->header.stamp = robot::Time::now();
action_goal->goal.target_pose = goal;
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::moveStraightTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
robot::log_info("[MoveBase::moveStraightTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
// Clear Order message since this is a non-Order moveStraightTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
if(controller_costmap_robot_ == nullptr)
{
robot::log_error("[MoveBase::moveStraightTo] controller_costmap_robot_ is null!");
return false;
}
robot::log_info("[MoveBase::moveStraightTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::moveStraightTo] Goal processed successfully by action server");
}
@@ -1240,19 +1483,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
if (!tc_)
{
throw std::runtime_error("Null 'tc_' pointer encountered");
robot::log_error("[MoveBase::rotateTo] tc_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null tc_");
nav_feedback_->goal_checked = false;
}
return false;
}
if (!controller_costmap_robot_)
{
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
robot::log_error("[MoveBase::rotateTo] controller_costmap_robot_ pointer is null");
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Internal error: null controller_costmap_robot_");
nav_feedback_->goal_checked = false;
}
return false;
}
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
try
{
if(!tc_->swapPlanner(rotate_planner_name_))
if (!tc_->swapPlanner(rotate_planner_name_))
{
robot::log_error("[MoveBase::rotateTo] Failed to swapPlanner");
lock.unlock();
return false;
}
}
@@ -1269,29 +1527,34 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
nav_feedback_->feed_back_str = std::string("Planning");
nav_feedback_->goal_checked = false;
}
else
{
robot::log_error("[MoveBase::rotateTo] nav_feedback_ pointer is null!");
lock.unlock();
return false;
}
if (cancel_ctr_)
cancel_ctr_ = false;
// Check if action server exists
if (!as_)
{
robot::log_error("[MoveBase::moveTo] as_ pointer is null!");
robot::log_error("[MoveBase::rotateTo] as_ pointer is null!");
lock.unlock();
return false;
}
try
{
robot_geometry_msgs::Pose2D pose;
if (!this->getRobotPose(pose))
{
if (nav_feedback_)
{
nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED;
nav_feedback_->feed_back_str = std::string("Coudn't get robot pose");
nav_feedback_->feed_back_str = std::string("Couldn't get robot pose");
nav_feedback_->goal_checked = false;
}
lock.unlock();
return false;
}
@@ -1302,23 +1565,22 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
action_goal->goal.target_pose.pose.position.x = pose.x + distance * cos(pose.theta);
action_goal->goal.target_pose.pose.position.y = pose.y + distance * sin(pose.theta);
// Generate unique goal ID using timestamp
robot::Time now = robot::Time::now();
action_goal->goal_id.stamp = now;
std::ostringstream goal_id_stream;
goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec;
action_goal->goal_id.id = goal_id_stream.str();
robot::log_info("[MoveBase::rotateTo] Generated goal ID: %s", action_goal->goal_id.id.c_str());
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
robot::log_info("[MoveBase::rotateTo] Goal stamp: %ld.%09ld",
action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec);
// Clear Order message since this is a non-Order moveTo call
// Clear Order message since this is a non-Order rotateTo call
{
boost::unique_lock<boost::recursive_mutex> planner_lock(planner_mutex_);
planner_order_.reset();
}
robot::log_info("[MoveBase::rotateTo] Processing goal through action server...");
as_->processGoal(action_goal);
robot::log_info("[MoveBase::rotateTo] Goal processed successfully by action server");
@@ -1326,7 +1588,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
catch (const std::exception &e)
{
lock.unlock();
robot::log_error("[MoveBase::moveStraightTo] Exception during processGoal: %s", e.what());
robot::log_error("[MoveBase::rotateTo] Exception during processGoal: %s", e.what());
return false;
}
@@ -1459,7 +1721,6 @@ bool move_base::MoveBase::setTwistLinear(const robot_geometry_msgs::Vector3 &lin
{
if (tc_->islock())
return true;
return tc_->setTwistLinear(linear);
}
else if (tc_ && cancel_ctr_)
@@ -1878,7 +2139,7 @@ void move_base::MoveBase::planThread()
while (wait_for_wake || !runPlanner_)
{
// if we should not be running the planner then suspend this thread
std::cout << "Planner thread is suspending" << std::endl;
robot::log_info("Planner thread is suspending");
planner_cond_.wait(lock);
wait_for_wake = false;
}
@@ -1888,7 +2149,7 @@ void move_base::MoveBase::planThread()
robot_geometry_msgs::PoseStamped temp_goal = planner_goal_;
boost::shared_ptr<robot_protocol_msgs::Order> temp_order = planner_order_;
lock.unlock();
std::cout << "Planning..." << std::endl;
robot::log_info("Planning...");
// run planner
planner_plan_->clear();
// ROS_INFO("MakePlan goal %f %f", temp_goal.pose.position.x, temp_goal.pose.position.y);
@@ -1910,7 +2171,7 @@ void move_base::MoveBase::planThread()
if (gotPlan)
{
std::cout << "Got Plan with " << planner_plan_->size() << " points!" << std::endl;
robot::log_info("Got Plan with %d points!", planner_plan_->size());
// pointer swap the plans under mutex (the controller will pull from latest_plan_)
std::vector<robot_geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
@@ -1921,7 +2182,7 @@ void move_base::MoveBase::planThread()
planning_retries_ = 0;
new_global_plan_ = true;
std::cout << "Generated a plan from the base_global_planner" << std::endl;
robot::log_info("Generated a plan from the base_global_planner");
// make sure we only start the controller if we still haven't reached the goal
if (runPlanner_)
@@ -1937,7 +2198,7 @@ void move_base::MoveBase::planThread()
// if we didn't get a plan and we are in the planning state (the robot isn't moving)
else if (state_ == move_base::PLANNING)
{
std::cout << "No Plan..." << std::endl;
robot::log_info("No Plan...");
robot::Time attempt_end = last_valid_plan_ + robot::Duration(planner_patience_);
// check if we've tried to make a plan for over our time limit or our maximum number of retries