diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 121d050..5a12570 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -789,16 +789,16 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( // (is_stopped || sign(angle_to_path) * sign_x < 0 ) && fabs(angle_to_path) > heading_rotate; bool result = use_rotate_to_heading_ && fabs(angle_to_path) > heading_rotate; -#ifdef BUILD_WITH_ROS - if (result) - ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); +// #ifdef BUILD_WITH_ROS + // if (result) + // ROS_WARN_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // else if(fabs(velocity.x) < min_speed_xy_) // { // ROS_INFO_THROTTLE(0.1, "velocity.x: %f, velocity.theta: %f, ", velocity.x, velocity.theta); // ROS_INFO_THROTTLE(0.1, "angle_to_path: %f, heading_rotate: %f, is_stopped: %x %x, sign_x: %f", angle_to_path, heading_rotate, is_stopped, sign(angle_to_path) * sign_x < 0, sign_x); // } -#endif +// #endif return result; } diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner index 56d05e4..d51ecc0 160000 --- a/src/Algorithms/Packages/global_planners/dock_planner +++ b/src/Algorithms/Packages/global_planners/dock_planner @@ -1 +1 @@ -Subproject commit 56d05e4322a9ef57c7e421d86d244f764edefcfe +Subproject commit d51ecc0986a5ebe3ed728477bd2fa0aabbba85da diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index eac6f13..0953730 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -36,8 +36,6 @@ namespace two_points_planner if (!initialized_) { robot::NodeHandle nh_priv_("~/" + name); - robot::log_info("TwoPointsPlanner: Name is %s", name.c_str()); - int lethal_obstacle; nh_priv_.getParam("lethal_obstacle", lethal_obstacle, 20); lethal_obstacle_ = (unsigned char)lethal_obstacle; @@ -121,6 +119,7 @@ namespace two_points_planner robot::log_error("[%s:%d]\n TwoPointsPlanner: Global planner is not initialized", __FILE__, __LINE__); return false; } + robot_nav_2d_msgs::Pose2DStamped start_2d = robot_nav_2d_utils::poseStampedToPose2D(start); robot_nav_2d_msgs::Pose2DStamped goal_2d = robot_nav_2d_utils::poseStampedToPose2D(goal); robot::log_info_at(__FILE__, __LINE__, "Making plan from start (%.2f,%.2f,%.2f) to goal (%.2f,%.2f,%.2f)", @@ -148,21 +147,21 @@ namespace two_points_planner } plan.clear(); - plan.push_back(start); + // plan.push_back(start); - unsigned int mx_start, my_start; - unsigned int mx_end, my_end; - if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, - start.pose.position.y, - mx_start, my_start) + // unsigned int mx_start, my_start; + // unsigned int mx_end, my_end; + // if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x, + // start.pose.position.y, + // mx_start, my_start) - || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, - goal.pose.position.y, - mx_end, my_end)) - { - robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__); - return false; - } + // || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x, + // goal.pose.position.y, + // mx_end, my_end)) + // { + // robot::log_error("[%s:%d]\n TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'", __FILE__, __LINE__); + // return false; + // } // unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start)); // unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end)); // if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 7a59917..51e318f 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -250,11 +250,16 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() if(rotate_algorithm_) rotate_algorithm_->reset(); ret_nav_ = ret_angle_ = false; + robot::log_info_at(__FILE__, __LINE__, "Debug"); + parent_.printParams(); std::string algorithm_nav_name; planner_nh_.param("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA")); + // parent_.setParam(algorithm_nav_name, original_papams_); robot::NodeHandle nh_algorithm = robot::NodeHandle(parent_, algorithm_nav_name); - parent_.setParam(algorithm_nav_name, original_papams_); nh_algorithm.setParam("allow_rotate", false); + + robot::log_info_at(__FILE__, __LINE__, "Debug ở đây"); + parent_.printParams(); } void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) @@ -567,17 +572,50 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_ } pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::DockingPlanner(const std::string &name) - : initialized_(false), is_goal_reached_(false), delayed_(false), detected_timeout_(false), - is_detected_(false), name_(name), docking_planner_name_(""), docking_nav_name_("") + : initialized_(false), + is_detected_(false), + is_goal_reached_(false), + following_(false), + allow_rotate_(false), + xy_goal_tolerance_(0.05), + yaw_goal_tolerance_(0.05), + min_lookahead_dist_(0.4), + max_lookahead_dist_(1.0), + lookahead_time_(1.5), + angle_threshold_(0.4), + name_(name), + docking_planner_name_(), + docking_nav_name_(), + docking_planner_(nullptr), + docking_nav_(nullptr), + linear_(), + angular_(), + nh_(), + nh_priv_(), + tf_(nullptr), + costmap_robot_(nullptr), + traj_generator_(), + docking_planner_loader_(), + docking_nav_loader_(), + detected_timeout_wt_(), + delayed_wt_(), + delayed_(false), + detected_timeout_(false), + robot_base_frame_(), + maker_goal_frame_() { } pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner() { - if (docking_planner_) + detected_timeout_wt_.stop(); + delayed_wt_.stop(); + if (docking_planner_ != nullptr) { docking_planner_.reset(); - if (docking_nav_) + } + if (docking_nav_ != nullptr) { docking_nav_.reset(); + } } void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 962be7f..753985c 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -107,6 +107,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, { robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath(algorithm_nav_name); + robot::log_info_at(__FILE__, __LINE__, "path_file_so: %s", path_file_so.c_str()); nav_algorithm_loader_ = boost::dll::import_alias( path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations); nav_algorithm_ = nav_algorithm_loader_(); diff --git a/src/Libraries/laser_geometry b/src/Libraries/laser_geometry index 50062ef..7e70a03 160000 --- a/src/Libraries/laser_geometry +++ b/src/Libraries/laser_geometry @@ -1 +1 @@ -Subproject commit 50062ef54970ffb6a88f550cfd7dac7a6d587041 +Subproject commit 7e70a03bc004074075bb03db66e59b75bf3f19f5 diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index 7d38393..2b70a79 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -651,7 +651,8 @@ namespace robot std::string key = it->first.as(); const YAML::Node &value = it->second; std::string full_key = prefix.empty() ? key : prefix + "/" + key; - + if(full_key.find("MKTAlgorithmDiffPredictiveTrajectory") == std::string::npos) + continue; if (value.IsMap()) { std::cout << "[NodeHandle] " << indent << full_key << ":" << std::endl; diff --git a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp index a109fab..ea1dbe8 100644 --- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp +++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp @@ -66,6 +66,7 @@ std::string PluginLoaderHelper::findLibraryPath(const std::string& symbol_name) } // Try to read from NodeHandle std::string library_path; + robot::log_info_at(__FILE__, __LINE__, "%s", symbol_name.c_str()); if (nh_.hasParam(param_path)) { nh_.getParam(param_path, library_path, std::string("")); if (!library_path.empty()) { diff --git a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h index 5373293..09dfc71 100755 --- a/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace robot_nav_core @@ -148,6 +149,12 @@ namespace robot_nav_core */ virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0; + /** + * @brief Set the odometry of the robot + * @param odom The odometry of the robot + */ + virtual void setOdom(robot_nav_msgs::Odometry *odom) { odom_ = odom; } + /** * @brief Virtual destructor for the interface */ @@ -155,6 +162,11 @@ namespace robot_nav_core protected: BaseLocalPlanner() {} + + /** + * @brief The odometry of the robot + */ + robot_nav_msgs::Odometry *odom_; }; } // namespace robot_nav_core diff --git a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h index 0bb1184..8bd4e12 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h @@ -189,11 +189,6 @@ namespace robot_nav_core_adapter robot_nav_2d_msgs::Pose2DStamped last_goal_; bool has_active_goal_; - /** - * @brief The odometry of the robot - */ - robot_nav_msgs::Odometry odom_; - // Plugin handling boost::function planner_loader_; robot_nav_core2::LocalPlanner::Ptr planner_; diff --git a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index 3126c0e..e3b82e9 100755 --- a/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -88,7 +88,7 @@ namespace robot_nav_core_adapter private_nh_ = robot::NodeHandle("~"); adapter_nh_ = robot::NodeHandle(private_nh_, name); - + std::string planner_name; if (adapter_nh_.hasParam("planner_name")) { @@ -291,7 +291,7 @@ namespace robot_nav_core_adapter robot_geometry_msgs::Twist LocalPlannerAdapter::getActualTwist() { - return odom_.twist.twist; + return odom_->twist.twist; } bool LocalPlannerAdapter::islock() @@ -356,7 +356,7 @@ namespace robot_nav_core_adapter if (!getRobotPose(pose2d)) return false; - robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_.twist.twist); + robot_nav_2d_msgs::Twist2D velocity = robot_nav_2d_utils::twist3Dto2D(odom_->twist.twist); bool ret = planner_->isGoalReached(pose2d, velocity); if (ret) { diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index dcec1de..2c0185c 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -324,6 +324,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) throw std::runtime_error("Failed to load local planner " + local_planner); } tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_); + tc_->setOdom(&odometry_); } catch (const std::exception &ex) { @@ -1781,32 +1782,6 @@ void move_base::MoveBase::cancel() boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); cancel_ctr_ = true; robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true"); - - if (as_) - { - // Get current goal ID from action server to cancel specific goal - // If we want to cancel all goals, use empty string "" - robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); - - // Use empty string to cancel current goal (processCancel accepts empty string to cancel all) - msg->id = ""; // Empty string cancels current goal - msg->stamp = robot::Time::now(); - - robot::log_info("[MoveBase::cancel] Sending cancel request to action server"); - robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", - msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec); - - // Convert to ConstPtr for processCancel - robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; - as_->processCancel(cancel_msg); - robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); - } - else - { - robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); - } - - robot::log_info("[MoveBase::cancel] ===== EXIT ====="); } bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose) @@ -2381,8 +2356,9 @@ void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalCons robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y); } } - + robot::log_warning("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y); robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); + robot::log_warning("Received goalToGlobalFrame: x=%.2f, y=%.2f", goal.pose.position.x, goal.pose.position.y); publishZeroVelocity(); // we have a goal so start the planner boost::unique_lock lock(planner_mutex_); @@ -2602,7 +2578,6 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio robot::log_error("Quaternion has nans or infs... discarding as a navigation goal"); return false; } - tf3::Quaternion tf_q(q.x, q.y, q.z, q.w); // next, we need to check if the length of the quaternion is close to zero @@ -2764,6 +2739,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) recovery_index_ = 0; } + // Cancle executeCycle if (cancel_ctr_ && tc_) { @@ -2772,13 +2748,25 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) tc_->setTwistLinear(linear); try { - if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_) + double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2)); + if (actual_linear_velocity <= min_approach_linear_velocity_) { robot::log_info("MoveTo is canceled."); resetState(); + cancel_ctr_ = false; // if(default_config_.base_global_planner != last_config_.base_global_planner) - swapPlanner(default_config_.base_global_planner); - + robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_); + if(nh_planner.hasParam("base_global_planner")) + { + std::string base_global_planner; + nh_planner.getParam("base_global_planner", base_global_planner); + swapPlanner(base_global_planner); + } + else + { + swapPlanner(default_config_.base_global_planner); + } + cancel_ctr_ = false; // disable the planner thread boost::unique_lock lock(planner_mutex_); runPlanner_ = false; @@ -2789,7 +2777,21 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED; } // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled."); - cancel_ctr_ = false; + + if (as_) + { + robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); + msg->id = ""; // Empty string cancels current goal + msg->stamp = robot::Time::now(); + // Convert to ConstPtr for processCancel + robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; + as_->processCancel(cancel_msg); + robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); + } + else + { + robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); + } return true; } } @@ -2798,7 +2800,17 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) robot::log_info("MoveTo is canceled."); resetState(); // if(default_config_.base_global_planner != last_config_.base_global_planner) - swapPlanner(default_config_.base_global_planner); + robot::NodeHandle nh_planner = robot::NodeHandle(private_nh_, position_planner_name_); + if(nh_planner.hasParam("base_global_planner")) + { + std::string base_global_planner; + nh_planner.getParam("base_global_planner", base_global_planner); + swapPlanner(base_global_planner); + } + else + { + swapPlanner(default_config_.base_global_planner); + } // disable the planner thread boost::unique_lock lock(planner_mutex_); @@ -2810,6 +2822,20 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTED; } // as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Canled."); + if (as_) + { + robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); + msg->id = ""; // Empty string cancels current goal + msg->stamp = robot::Time::now(); + // Convert to ConstPtr for processCancel + robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; + as_->processCancel(cancel_msg); + robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); + } + else + { + robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); + } cancel_ctr_ = false; return true; } @@ -2830,7 +2856,6 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) // if we're controlling, we'll attempt to find valid velocity commands case move_base::CONTROLLING: - // robot::log_debug("In controlling state."); // check to see if we've reached our goal try @@ -3004,7 +3029,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) tc_->setTwistLinear(linear); try { - if (fabs(tc_->getActualTwist().linear.x) <= min_approach_linear_velocity_) + double actual_linear_velocity = sqrt(std::pow(tc_->getActualTwist().linear.x, 2) + std::pow(tc_->getActualTwist().linear.y, 2)); + if (actual_linear_velocity <= min_approach_linear_velocity_) { paused_ = true; } @@ -3108,15 +3134,48 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) robot_geometry_msgs::PoseStamped move_base::MoveBase::goalToGlobalFrame(const robot_geometry_msgs::PoseStamped &goal_pose_msg) { - std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); - robot_geometry_msgs::PoseStamped goal_pose, global_pose; - goal_pose = goal_pose_msg; - goal_pose.header.stamp = robot::Time(); // latest available + if (!tf_) + { + return goal_pose_msg; + } + + std::string global_frame = planner_costmap_robot_->getGlobalFrameID(); + // robot_geometry_msgs::PoseStamped goal_pose, global_pose; + // goal_pose = goal_pose_msg; + + // goal_pose.header.stamp = robot::Time(); // latest available + // try + // { + // tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); + // tf3::doTransform(goal_pose, global_pose, transform); + // } + robot_geometry_msgs::PoseStamped global_pose; + tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose); + robot_geometry_msgs::PoseStamped goal_pose; + tf3::toMsg(tf3::Transform::getIdentity(), goal_pose.pose); + goal_pose = goal_pose_msg; + robot::Time current_time = robot::Time::now(); // save time for checking tf delay later + tf3::Time tf3_current_time = data_convert::convertTime(current_time); + tf3::Time tf3_zero_time = data_convert::convertTime(robot::Time()); + try { - tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3::Time()); - tf3::doTransform(goal_pose, global_pose, transform); + // use current time if possible (makes sure it's not in the future) + std::string error_msg; + if (tf_->canTransform(global_frame, goal_pose.header.frame_id, tf3_current_time, &error_msg)) + { + // Transform is available at current time + tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_current_time); + tf3::doTransform(goal_pose, global_pose, transform); + } + // use the latest otherwise (tf3::Time() means latest available) + else + { + // Try to get latest transform + tf3::TransformStampedMsg transform = tf_->lookupTransform(global_frame, goal_pose.header.frame_id, tf3_zero_time); + tf3::doTransform(goal_pose, global_pose, transform); + } } catch (tf3::LookupException &ex) {