diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index a19ea5a..8e0a7b5 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -201,7 +201,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs { sub_goal_index = (unsigned int)index_s.front() > 0 ? (unsigned int)index_s.front() - 1 : (unsigned int)index_s.front(); sub_goal_seq_saved_ = global_plan.poses[sub_goal_index].header.seq; - std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl; + // std::cout << "ScoreAlgorithm: Updated sub_goal_index to " << sub_goal_index << " based on index_s.front()" << std::endl; } // Process index_s with multiple elements 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 3055289..70a6a79 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 @@ -790,7 +790,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf robot_nav_2d_msgs::Pose2DStamped robot; if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) { - throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); } double dist_thresh_sq = dist_behind_robot * dist_behind_robot; @@ -855,7 +855,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( robot_nav_2d_msgs::Pose2DStamped robot_pose; if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) { - throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + throw robot_nav_core2::PlannerTFException("PredictiveTrajectory: Unable to transform robot pose into global plan's frame"); } // we'll discard points on the plan that are outside the local costmap 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 0fcae10..6749fbc 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 @@ -226,15 +226,16 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; + // robot::log_info("pose: %f %f %f", pose.pose.x, pose.pose.y, pose.pose.theta); robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), local_goal_pose = this->transformPoseToLocal(goal_pose_); - - if (pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_)) + // 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_)) { - robot::log_debug_at(__FILE__, __LINE__, "Transform global plan is successful"); - } - else robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed"); + throw robot_nav_core2::LocalPlannerException("Transform global plan is failed"); + } double x_direction, y_direction, theta_direction; if (!ret_nav_) @@ -276,6 +277,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeV } catch (const robot_nav_core2::PlannerException &e) { + robot::log_warning_at(__FILE__, __LINE__, "PNKXLocalPlanner computing velocity commands has been broken: %s", e.what()); throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken"); return cmd_vel; } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp index a95751a..ef1a911 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp @@ -111,7 +111,7 @@ bool pnkx_local_planner::transformGlobalPlan( robot_nav_2d_msgs::Pose2DStamped robot_pose; if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) { - throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); + throw robot_nav_core2::PlannerTFException("transformGlobalPlan: Unable to transform robot pose into global plan's frame"); } transformed_plan.header.frame_id = costmap->getGlobalFrameID(); @@ -172,8 +172,8 @@ bool pnkx_local_planner::transformGlobalPlan( // Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution if (pnkx_local_planner::getSquareDistance(costmap_pose.pose, w.pose) < sq_prune_dist) { - robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n", - costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y); + // robot::log_debug_at(__FILE__, __LINE__, "Nearest waypoint to <%f, %f> is <%f, %f>\n", + // costmap_pose.pose.x, costmap_pose.pose.y, w.pose.x, w.pose.y); break; } diff --git a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt index db7ba10..13df305 100755 --- a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt @@ -37,7 +37,7 @@ if (NOT BUILDING_WITH_CATKIN) tf3 robot_tf3_geometry_msgs robot_cpp - console_bridge + data_convert ) else() diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h index 18d14d7..313ab48 100755 --- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h @@ -42,6 +42,18 @@ namespace robot_nav_2d_utils { + + /** + * @brief Transform a PoseStamped from one frame to another + * + * @param tf Smart pointer to TFListener + * @param in_pose Pose to transform + * @param out_pose Place to store the resulting transformed pose + * @param frame Frame to transform the pose into + * @return True if successful transform + */ + bool transform(const TFListenerPtr tf, const ::robot_geometry_msgs::PoseStamped &in_pose,::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame); + /** * @brief Transform a PoseStamped from one frame to another while catching exceptions * diff --git a/src/Libraries/robot_nav_2d_utils/package.xml b/src/Libraries/robot_nav_2d_utils/package.xml index 2549adb..cbdf32f 100755 --- a/src/Libraries/robot_nav_2d_utils/package.xml +++ b/src/Libraries/robot_nav_2d_utils/package.xml @@ -25,11 +25,13 @@ nav_grid robot_nav_core2 robot_cpp - + data_convert + robot_nav_2d_msgs robot_geometry_msgs robot_nav_msgs nav_grid robot_nav_core2 robot_cpp + data_convert \ No newline at end of file diff --git a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp index 70282f7..6e8f8ed 100755 --- a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp @@ -31,6 +31,8 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include #include #include #include @@ -105,7 +107,7 @@ namespace robot_nav_2d_utils pose2d.header = pose.header; pose2d.pose.x = pose.pose.position.x; pose2d.pose.y = pose.pose.position.y; - // pose2d.pose.theta = tf::getYaw(pose.pose.orientation); + pose2d.pose.theta = data_convert::getYaw(pose.pose.orientation); return pose2d; } @@ -114,7 +116,7 @@ namespace robot_nav_2d_utils robot_geometry_msgs::Pose pose; pose.position.x = pose2d.x; pose.position.y = pose2d.y; - // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); + pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta); return pose; } @@ -134,7 +136,7 @@ namespace robot_nav_2d_utils pose.header.stamp = stamp; pose.pose.position.x = pose2d.x; pose.pose.position.y = pose2d.y; - // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); + pose.pose.orientation = data_convert::createQuaternionMsgFromYaw(pose2d.theta); return pose; } @@ -284,10 +286,10 @@ namespace robot_nav_2d_utils info.height = metadata.height; info.origin_x = metadata.origin.position.x; info.origin_y = metadata.origin.position.y; - // if (std::abs(tf::getYaw(metadata.origin.orientation)) > 1e-5) - // { - // std::cout << "Conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring" << std::endl; - // } + if (std::abs(data_convert::getYaw(metadata.origin.orientation)) > 1e-5) + { + robot::log_warning("[conversions] conversion from MapMetaData to NavGridInfo encountered a non-zero rotation. Ignoring"); + } return info; } diff --git a/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp index 136601a..13fc20b 100755 --- a/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp @@ -41,36 +41,54 @@ namespace robot_nav_2d_utils { + + bool transform(const TFListenerPtr tf, + const ::robot_geometry_msgs::PoseStamped &in_pose, + ::robot_geometry_msgs::PoseStamped &out_pose, const ::std::string &frame) + { + const std::string target_frame = frame; + const std::string source_frame = in_pose.header.frame_id; + std::string *error_msg; + + if(tf->canTransform(target_frame, source_frame, tf3::Time(), error_msg)) + { + tf3::TransformStampedMsg transform =tf->lookupTransform(target_frame, source_frame, tf3::Time()); + tf3::doTransform(in_pose, out_pose, transform); + return true; + } + return false; + } + bool transformPose(const TFListenerPtr tf, const ::std::string frame, const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose, const bool extrapolation_fallback) { - // if (in_pose.header.frame_id == frame) - // { - // out_pose = in_pose; - // return true; - // } + if (in_pose.header.frame_id == frame) + { + out_pose = in_pose; + return true; + } - // try - // { - // tf->transform(in_pose, out_pose, frame); - // return true; - // } - // catch (tf3::ExtrapolationException &ex) - // { - // if (!extrapolation_fallback) - // throw; - // robot_geometry_msgs::PoseStamped latest_in_pose; - // latest_in_pose.header.frame_id = in_pose.header.frame_id; - // latest_in_pose.pose = in_pose.pose; - // tf->transform(latest_in_pose, out_pose, frame); - // return true; - // } - // catch (tf3::TransformException &ex) - // { - // robot::log_error("Exception in transformPose: %s", ex.what()); - // return false; - // } + try + { + transform(tf, in_pose, out_pose, frame); + return true; + } + catch (tf3::ExtrapolationException &ex) + { + if (!extrapolation_fallback) + throw; + robot_geometry_msgs::PoseStamped latest_in_pose; + latest_in_pose.header.frame_id = in_pose.header.frame_id; + latest_in_pose.pose = in_pose.pose; + transform(tf, latest_in_pose, out_pose, frame); + return true; + } + catch (tf3::TransformException &ex) + { + robot::log_error("Exception in transformPose: %s", ex.what()); + return false; + } return false; } 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 65db055..c4671a8 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 @@ -188,7 +188,7 @@ namespace robot_nav_core_adapter } catch (const robot_nav_core2::PlannerException &e) { - std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl; + robot::log_error_at(__FILE__, __LINE__, "LocalPlannerAdapter computeVelocityCommands exception: %s", e.what()); return false; } cmd_vel = robot_nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity); diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 36b4457..91ef2b5 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -1246,6 +1246,10 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal, robot::log_error("Failed to find a plan without Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y); return false; } + else + { + global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan); + } lock.unlock(); return true; } @@ -1280,6 +1284,10 @@ bool move_base::MoveBase::makePlan(const robot_protocol_msgs::Order &msg, const robot::log_error("Failed to find a plan with Order to point (%f, %f)", goal.pose.position.x, goal.pose.position.y); return false; } + else + { + global_data_.plan = robot_nav_2d_utils::posesToPath2D(plan); + } lock.unlock(); return true; } @@ -2136,7 +2144,7 @@ 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."); + // robot::log_debug("In controlling state."); // check to see if we've reached our goal try @@ -2198,12 +2206,8 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) { if (tc_->computeVelocityCommands(odometry_.twist.twist, cmd_vel)) { - robot::log_debug("Got a valid command from the local planner: %.3lf, %.3lf, %.3lf", - cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z); - last_valid_control_ = robot::Time::now(); // make sure that we send the velocity command to the base - // vel_pub_.publish(cmd_vel); if (nav_feedback_) { @@ -2216,8 +2220,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) } else { - robot::log_debug("The local planner could not find a valid plan."); - robot::log_warning("The local planner could not find a valid plan."); + robot::log_warning_at(__FILE__, __LINE__, "The local planner could not find a valid plan."); robot::Time attempt_end = last_valid_control_ + robot::Duration(controller_patience_); // check if we've tried to find a valid control for longer than our time limit @@ -2246,34 +2249,34 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) // enable the planner thread in case it isn't running on a clock boost::unique_lock lock(planner_mutex_); - // if (as_->isPreemptRequested()) - // { - // if (as_->isNewGoalAvailable()) - // { - // // if we're active and a new goal is available, we'll accept it, but we won't shut anything down - // move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); - // if (!isQuaternionValid(new_goal.target_pose.pose.orientation)) - // { - // if (nav_feedback_) - // { - // nav_feedback_->navigation_state = move_base_core::State::ABORTED; - // nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion"); - // } - // as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); - // return false; - // } - // else - // { - // if (nav_feedback_) - // { - // nav_feedback_->navigation_state = move_base_core::State::ACTIVE; - // nav_feedback_->feed_back_str = std::string("The new goal is received"); - // } - // } - // goal = goalToGlobalFrame(new_goal.target_pose); - // planner_goal_ = goal; - // } - // } + if (as_->isPreemptRequested()) + { + if (as_->isNewGoalAvailable()) + { + // if we're active and a new goal is available, we'll accept it, but we won't shut anything down + robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); + if (!isQuaternionValid(new_goal.target_pose.pose.orientation)) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; + nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion"); + } + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); + return false; + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE; + nav_feedback_->feed_back_str = std::string("The new goal is received"); + } + } + goal = goalToGlobalFrame(new_goal.target_pose); + planner_goal_ = goal; + } + } runPlanner_ = true; cancel_ctr_ = false; planner_cond_.notify_one(); @@ -2311,21 +2314,13 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) } break; - // we'll try to clear out space with any user-provided recovery behaviors + // we'll try to clear out space with any user-provided recovery behaviors case move_base::CLEARING: robot::log_debug("[%s:%d] In clearing/recovery state", __FILE__, __LINE__); // we'll invoke whatever recovery behavior we're currently on if they're enabled if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()) { robot::log_debug("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size()); - - // move_base_msgs::RecoveryStatus msg; - // msg.pose_stamped = current_position; - // msg.current_recovery_number = recovery_index_; - // msg.total_number_of_recoveries = recovery_behaviors_.size(); - // msg.recovery_behavior_name = recovery_behavior_names_[recovery_index_]; - // recovery_status_pub_.publish(msg); - recovery_behaviors_[recovery_index_]->runBehavior(); // we at least want to give the robot some time to stop oscillating after executing the behavior @@ -2363,7 +2358,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; nav_feedback_->feed_back_str = std::string("Failed to find a valid control. Even after executing recovery behaviors."); } - // as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); } else if (recovery_trigger_ == PLANNING_R) { @@ -2373,7 +2368,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; nav_feedback_->feed_back_str = "Failed to find a valid plan. Even after executing recovery behaviors."; } - // as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); } else if (recovery_trigger_ == OSCILLATION_R) { @@ -2383,7 +2378,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; nav_feedback_->feed_back_str = std::string("Robot is oscillating. Even after executing recovery behaviors."); } - // as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); } return true; } @@ -2400,7 +2395,7 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; nav_feedback_->feed_back_str = std::string("Reached a case that should not be hit in move_base. This is a bug, please report it."); } - // as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); return true; }