diff --git a/CMakeLists.txt b/CMakeLists.txt index 2668be0..e4fb638 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,16 +66,16 @@ if (NOT TARGET voxel_grid) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid) endif() -if (NOT TARGET nav_2d_msgs) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs) +if (NOT TARGET robot_nav_2d_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_msgs) endif() if (NOT TARGET costmap_2d) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/costmap_2d) endif() -if (NOT TARGET nav_2d_utils) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_utils) +if (NOT TARGET robot_nav_2d_utils) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) endif() if (NOT TARGET nav_core) @@ -98,57 +98,57 @@ if (NOT TARGET move_base_core) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/move_base_core) endif() -if (NOT TARGET mkt_msgs) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs) -endif() +# if (NOT TARGET mkt_msgs) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_msgs) +# endif() -if (NOT TARGET angles) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles) -endif() +# if (NOT TARGET angles) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/angles) +# endif() -if (NOT TARGET kalman) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman) -endif() +# if (NOT TARGET kalman) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/kalman) +# endif() -if (NOT TARGET score_algorithm) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm) -endif() +# if (NOT TARGET score_algorithm) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm) +# endif() -if (NOT TARGET mkt_plugins) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins) -endif() +# if (NOT TARGET mkt_plugins) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_plugins) +# endif() -if (NOT TARGET mkt_algorithm) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) -endif() +# if (NOT TARGET mkt_algorithm) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm) +# endif() -if (NOT TARGET two_points_planner) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner) -endif() +# if (NOT TARGET two_points_planner) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner) +# endif() -if (NOT TARGET custom_planner) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner) -endif() +# if (NOT TARGET custom_planner) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/custom_planner) +# endif() -if (NOT TARGET dock_planner) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner) -endif() +# if (NOT TARGET dock_planner) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/dock_planner) +# endif() -if (NOT TARGET pnkx_local_planner) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) -endif() +# if (NOT TARGET pnkx_local_planner) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) +# endif() # 2. Main packages (phụ thuộc vào cores) message(STATUS "[move_base] Shared library configured") -if (NOT TARGET move_base) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) -endif() +# if (NOT TARGET move_base) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Packages/move_base) +# endif() # C API for .NET/C# integration -if (NOT TARGET navigation_c_api) - add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) -endif() +# if (NOT TARGET navigation_c_api) +# add_subdirectory(${CMAKE_SOURCE_DIR}/src/APIs/c_api) +# endif() message(STATUS "========================================") message(STATUS "All packages configured successfully") diff --git a/doc/architecture_discussion.md b/doc/architecture_discussion.md index 7de5531..fefb5c2 100644 --- a/doc/architecture_discussion.md +++ b/doc/architecture_discussion.md @@ -346,7 +346,7 @@ Cần làm rõ: 4. ✅ **Costmap Layer**: Global và local costmap với layers 5. ✅ **Algorithms Layer**: MKT algorithms, score algorithm, kalman 6. ✅ **API Layer**: C API wrapper cho .NET integration -7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, nav_2d_utils +7. ✅ **Supporting Libraries**: tf3, robot_time, geometry_msgs, robot_nav_2d_utils **Đang triển khai / Cần bổ sung:** ⚠️ diff --git a/doc/implementation_plan.md b/doc/implementation_plan.md index e4c7ea7..0c6391b 100644 --- a/doc/implementation_plan.md +++ b/doc/implementation_plan.md @@ -22,7 +22,7 @@ - `tf3` - Transform system - `robot_time` - Time management - `geometry_msgs` - Message types - - `nav_2d_msgs`, `nav_2d_utils` - 2D navigation utilities + - `robot_nav_2d_msgs`, `robot_nav_2d_utils` - 2D navigation utilities ### ❌ Cần bổ sung/hoàn thiện: diff --git a/setup_catkin_workspace.sh b/setup_catkin_workspace.sh index ee22c71..ce229e0 100755 --- a/setup_catkin_workspace.sh +++ b/setup_catkin_workspace.sh @@ -36,7 +36,7 @@ echo "Creating symlinks to packages with package.xml..." # Find all packages with package.xml in the source tree PACKAGES=( "src/Libraries/tf3" - "src/Libraries/nav_2d_utils" + "src/Libraries/robot_nav_2d_utils" "src/Navigations/Libraries/nav_grid" "src/Navigations/Cores/nav_core" "src/Navigations/Cores/nav_core2" diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 3b506a7..79f9ac4 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -449,8 +449,9 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou try { move_base_core::BaseNavigation* nav = static_cast(handle); - if (nav->nav_feedback_) { - cpp_to_c_nav_feedback(*nav->nav_feedback_, out_feedback); + move_base_core::NavFeedback cpp_feedback; + if (nav->getFeedback(cpp_feedback)) { + cpp_to_c_nav_feedback(cpp_feedback, out_feedback); return true; } return false; diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index a86045e..dd850bb 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -11,7 +11,7 @@ if(NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() -set(PACKAGES_DIR nav_2d_msgs nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles) +set(PACKAGES_DIR robot_nav_2d_msgs robot_nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles) find_package(Boost REQUIRED COMPONENTS filesystem system) diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h index dd78342..6432a34 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h @@ -5,9 +5,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include namespace score_algorithm @@ -48,8 +48,8 @@ namespace score_algorithm * @param velocity The robot's current velocity * @return True if goal is reached */ - virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose, - const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D& velocity) = 0; + virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& query_pose, const robot_nav_2d_msgs::Pose2DStamped& goal_pose, + const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D& velocity) = 0; }; } // namespace score_algorithm diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index 60504b2..b834a7e 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -3,9 +3,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -51,8 +51,8 @@ namespace score_algorithm * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, - const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, double &x_direction, double &y_direction, double &theta_direction) = 0; /** @@ -62,7 +62,7 @@ namespace score_algorithm * @param traj */ virtual mkt_msgs::Trajectory2D calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0; + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0; /** * @brief Reset all data @@ -99,7 +99,7 @@ namespace score_algorithm * @param last_valid_index * @return goal index */ - virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, + virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, const double distance, unsigned int start_index, unsigned int last_valid_index) const; /** @@ -111,8 +111,8 @@ namespace score_algorithm * @param S * @return true if goal pose is found, false otherwise */ - virtual bool computePlanCommand(const nav_2d_msgs::Pose2DStamped &robot_pose, const nav_2d_msgs::Twist2D &velocity, const double &S, - const nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, nav_2d_msgs::Path2D &result); + virtual bool computePlanCommand(const robot_nav_2d_msgs::Pose2DStamped &robot_pose, const robot_nav_2d_msgs::Twist2D &velocity, const double &S, + const robot_nav_2d_msgs::Path2D &global_plan, unsigned int &start_index_global_plan, unsigned int &goal_index_global_plan, robot_nav_2d_msgs::Path2D &result); /** * @brief Find sub goal index @@ -120,7 +120,7 @@ namespace score_algorithm * @param index_s * @return true if sub goal index is found, false otherwise */ - virtual bool findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes); + virtual bool findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes); /** * @brief Calculate journey @@ -129,7 +129,7 @@ namespace score_algorithm * @param last_valid_index * @return journey */ - double journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const; + double journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const; /** * @brief Check whether the robot is stopped or not @@ -138,7 +138,7 @@ namespace score_algorithm * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped * @return True if the robot is stopped, false otherwise */ - bool stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity); + bool stopped(const robot_nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity); void clear(); @@ -159,7 +159,7 @@ namespace score_algorithm // robot::Publisher closet_robot_goal_pub_; // robot::Publisher transformed_plan_pub_, compute_plan_pub_; - std::vector reached_intermediate_goals_; + std::vector reached_intermediate_goals_; std::vector start_index_saved_vt_; unsigned int sub_goal_index_saved_, sub_goal_seq_saved_; unsigned int sub_start_index_saved_, sub_start_seq_saved_; diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h index 09d0a93..5a6ca3d 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -3,9 +3,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -50,7 +50,7 @@ namespace score_algorithm * @brief Start a new iteration based on the current velocity * @param current_velocity */ - virtual void startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) = 0; + virtual void startNewIteration(const robot_nav_2d_msgs::Twist2D ¤t_velocity) = 0; /** * @brief Set direct of robot based on the current velocity @@ -96,7 +96,7 @@ namespace score_algorithm * @brief Return the next twist and advance the iteration * @return The Twist! */ - virtual nav_2d_msgs::Twist2D nextTwist() = 0; + virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0; /** * @brief Get all the twists for an iteration. @@ -106,9 +106,9 @@ namespace score_algorithm * @param current_velocity * @return all the twists */ - virtual std::vector getTwists(const nav_2d_msgs::Twist2D ¤t_velocity) + virtual std::vector getTwists(const robot_nav_2d_msgs::Twist2D ¤t_velocity) { - std::vector twists; + std::vector twists; startNewIteration(current_velocity); while (hasMoreTwists()) { @@ -123,10 +123,10 @@ namespace score_algorithm * @param start_vel Current robot velocity * @param cmd_vel The desired command velocity */ - virtual mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, - const nav_2d_msgs::Path2D &transformed_plan, - const nav_2d_msgs::Twist2D &start_vel, - const nav_2d_msgs::Twist2D &cmd_vel) = 0; + virtual mkt_msgs::Trajectory2D generateTrajectory(const robot_nav_2d_msgs::Pose2DStamped &start_pose, + const robot_nav_2d_msgs::Path2D &transformed_plan, + const robot_nav_2d_msgs::Twist2D &start_vel, + const robot_nav_2d_msgs::Twist2D &cmd_vel) = 0; virtual robot::NodeHandle getNodeHandle() = 0; diff --git a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp index 7c7400f..44e278d 100644 --- a/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp +++ b/src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp @@ -1,9 +1,9 @@ #include -#include -#include +#include +#include #include -unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, +unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector &plan, const double distance, unsigned int start_index, unsigned int last_valid_index) const { if (start_index >= last_valid_index) @@ -39,7 +39,7 @@ unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs:: return goal_index; } -bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes) +bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector &plan, std::vector &seq_s, std::vector &mutes) { if (plan.empty()) { @@ -50,7 +50,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vectorgetGlobalFrameID(); - nav_2d_msgs::Pose2DStamped p1, p2, p3; + robot_nav_2d_msgs::Pose2DStamped p1, p2, p3; if (plan.size() < 3) { @@ -80,7 +80,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector seq_s; - std::vector mutes; + std::vector mutes; if (!this->findSubGoalIndex(global_plan.poses, seq_s, mutes)) { std::cerr << "ScoreAlgorithm: Cannot find SubGoal Index" << std::endl; @@ -297,7 +297,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose for (unsigned int i = sub_start_index; i < sub_goal_index; ++i) { // Still on the costmap. Continue. - double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[i].pose, robot_pose.pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét + double distance = fabs(robot_nav_2d_utils::poseDistance(global_plan.poses[i].pose, robot_pose.pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét if (distance_to_start > distance) { start_index = i; @@ -375,9 +375,9 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose // int c = 0; for (auto &reached_intermediate_goal : reached_intermediate_goals_) { - double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose)); + double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose)); - // geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose); + // geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose); // poseArrayMsg.poses.push_back(pose); if (distance < xy_local_goal_tolerance_) @@ -387,7 +387,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose // (start_index is now > goal_index) for (start_index = goal_index; start_index < last_valid_index; ++start_index) { - distance = nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[start_index].pose); + distance = robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[start_index].pose); if (distance >= xy_local_goal_tolerance_) break; } @@ -398,7 +398,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose if (!goal_already_reached) { // new goal not in reached_intermediate_goals_ - double distance = fabs(nav_2d_utils::poseDistance(global_plan.poses[start_index].pose, global_plan.poses[goal_index].pose)); + double distance = fabs(robot_nav_2d_utils::poseDistance(global_plan.poses[start_index].pose, global_plan.poses[goal_index].pose)); if (distance < xy_local_goal_tolerance_) { // the robot has currently reached the goal @@ -429,10 +429,10 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose start_index_global_plan = start_index; goal_index_global_plan = goal_index; - nav_2d_msgs::Pose2DStamped sub_pose; + robot_nav_2d_msgs::Pose2DStamped sub_pose; sub_pose = global_plan.poses[closet_index]; - nav_2d_msgs::Pose2DStamped sub_goal; + robot_nav_2d_msgs::Pose2DStamped sub_goal; sub_goal = global_plan.poses[goal_index]; result.header = global_plan.header; @@ -443,20 +443,20 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose return true; } -double score_algorithm::ScoreAlgorithm::journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const +double score_algorithm::ScoreAlgorithm::journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const { double S = 0; if (last_valid_index - start_index > 1) { for (int i = start_index; i < last_valid_index; i++) { - S += nav_2d_utils::poseDistance(plan[i].pose, plan[i + 1].pose); + S += robot_nav_2d_utils::poseDistance(plan[i].pose, plan[i + 1].pose); } } return S; } -bool score_algorithm::ScoreAlgorithm::stopped(const nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity) +bool score_algorithm::ScoreAlgorithm::stopped(const robot_nav_2d_msgs::Twist2D &velocity, const double &rot_stopped_velocity, const double &trans_stopped_velocity) { return fabs(velocity.theta) <= rot_stopped_velocity && fabs(velocity.x) <= trans_stopped_velocity && fabs(velocity.y) <= trans_stopped_velocity; } diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 521e14c..92ebfa0 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -24,8 +24,8 @@ include_directories( set(PACKAGES_DIR geometry_msgs score_algorithm - nav_2d_msgs - nav_2d_utils + robot_nav_2d_msgs + robot_nav_2d_utils kalman angles nav_grid diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h index 7e33383..8860308 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h @@ -39,8 +39,8 @@ public: * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) override; /** @@ -53,7 +53,7 @@ public: * @param pose * @param velocity */ - virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override; protected: @@ -64,8 +64,8 @@ protected: double journey(const std::vector &plan, const unsigned int start_index, const unsigned int last_valid_index) const; - bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, - const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, + bool getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, + const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, double &target_direction); bool findSubGoalIndex(const std::vector &plan, std::vector &index_s); @@ -77,7 +77,7 @@ protected: std::vector reached_intermediate_goals_; geometry_msgs::Pose2D rev_sub_goal_, sub_goal_; geometry_msgs::Pose2D goal_; - nav_2d_msgs::Path2D global_plan_; + robot_nav_2d_msgs::Path2D global_plan_; unsigned int start_index_saved_; unsigned int sub_goal_index_saved_, sub_start_index_saved_; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h index e38182f..779e3c6 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h @@ -37,8 +37,8 @@ public: * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) override; /** @@ -51,7 +51,7 @@ public: * @param pose * @param velocity */ - virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override; protected: diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h index ee20972..2d09c42 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h @@ -29,7 +29,7 @@ public: * @return True if goal is reached */ bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose, - const std::vector& path, const nav_2d_msgs::Twist2D& velocity) override; + const std::vector& path, const robot_nav_2d_msgs::Twist2D& velocity) override; protected: diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h index 2902a79..5e20b6b 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h @@ -36,8 +36,8 @@ public: * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) override; /** @@ -45,7 +45,7 @@ public: * @param pose * @param velocity */ - virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override; protected: diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h index 6740036..fb6b86a 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h @@ -37,8 +37,8 @@ public: * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, + virtual bool prepare(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) override; /** @@ -51,7 +51,7 @@ public: * @param pose * @param velocity */ - virtual nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override; protected: diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h index c5ccfb8..1e43cee 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h @@ -33,7 +33,7 @@ namespace mkt_algorithm * @param traj */ virtual mkt_msgs::Trajectory2D calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new GoStraight instance diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 674c847..36e7730 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -18,10 +18,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace mkt_algorithm { @@ -56,8 +56,8 @@ namespace mkt_algorithm * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, - const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, double &x_direction, double &y_direction, double &theta_direction) override; /** @@ -67,7 +67,7 @@ namespace mkt_algorithm * @param traj */ virtual mkt_msgs::Trajectory2D calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Reset all data @@ -111,7 +111,7 @@ namespace mkt_algorithm * @param velocity * @return look ahead distance */ - double getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity); + double getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity); /** * @brief Get lookahead point on the global plan @@ -119,15 +119,15 @@ namespace mkt_algorithm * @param global_plan * @return lookahead point */ - std::vector::iterator - getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan); + std::vector::iterator + getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan); /** * @brief Create carrot message * @param carrot_pose * @return carrot message */ - geometry_msgs::PointStamped createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose); + geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose); /** * @brief Prune global plan @@ -137,8 +137,8 @@ namespace mkt_algorithm * @param dist_behind_robot * @return true if plan is pruned, false otherwise */ - bool pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, - nav_2d_msgs::Path2D &global_plan, double dist_behind_robot); + bool pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, + robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot); /** * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). @@ -156,9 +156,9 @@ namespace mkt_algorithm * @return \c true if the global plan is transformed, \c false otherwise */ bool transformGlobalPlan( - TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, - nav_2d_msgs::Path2D &transformed_plan); + robot_nav_2d_msgs::Path2D &transformed_plan); /** * @brief Should rotate to path @@ -167,7 +167,7 @@ namespace mkt_algorithm * @return true if should rotate, false otherwise */ bool shouldRotateToPath( - const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x); + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x); /** * @brief Rotate to heading @@ -175,7 +175,7 @@ namespace mkt_algorithm * @param velocity The velocity of the robot * @param cmd_vel The velocity commands to be filled */ - void rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel); + void rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); /** * @brief the robot is moving acceleration limits @@ -184,7 +184,7 @@ namespace mkt_algorithm * @param cmd_vel_result The velocity commands result */ void moveWithAccLimits( - const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result); + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result); /** * @brief Stop the robot taking into account acceleration limits @@ -193,7 +193,7 @@ namespace mkt_algorithm * @param cmd_vel The velocity commands to be filled * @return True if a valid trajectory was found, false otherwise */ - bool stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel); + bool stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel); /** * @brief Apply constraints @@ -207,7 +207,7 @@ namespace mkt_algorithm */ void applyConstraints( const double &dist_error, const double &lookahead_dist, - const double &curvature, const nav_2d_msgs::Twist2D &curr_speed, + const double &curvature, const robot_nav_2d_msgs::Twist2D &curr_speed, const double &pose_cost, double &linear_vel, double &sign_x); std::vector interpolateFootprint(geometry_msgs::Pose2D pose, std::vector footprint, double resolution); @@ -221,17 +221,17 @@ namespace mkt_algorithm double costAtPose(const double &x, const double &y); void updateCostAtOffset( - TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose, + TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, double x_offset, double y_offset, double &cost_left, double &cost_right); double computeDecelerationFactor(double remaining_distance, double decel_distance); - double getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); + double getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan); bool detectWobbleByCarrotAngle(std::vector& angle_history, double current_angle, double amplitude_threshold = 0.3, size_t window_size = 20); - void publishMarkers(nav_2d_msgs::Pose2DStamped pose); + void publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose); std::vector angle_history_; size_t window_size_; @@ -242,11 +242,11 @@ namespace mkt_algorithm std::string frame_id_path_; std::string robot_base_frame_; - nav_2d_msgs::Pose2DStamped goal_; - nav_2d_msgs::Path2D global_plan_; - nav_2d_msgs::Path2D compute_plan_; - nav_2d_msgs::Path2D transform_plan_; - nav_2d_msgs::Twist2D prevous_drive_cmd_; + robot_nav_2d_msgs::Pose2DStamped goal_; + robot_nav_2d_msgs::Path2D global_plan_; + robot_nav_2d_msgs::Path2D compute_plan_; + robot_nav_2d_msgs::Path2D transform_plan_; + robot_nav_2d_msgs::Twist2D prevous_drive_cmd_; double x_direction_, y_direction_, theta_direction_; double max_robot_pose_search_dist_; diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h index d4839d5..6a97482 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h @@ -35,8 +35,8 @@ namespace mkt_algorithm * @param goal The final goal (costmap frame) * @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points */ - virtual bool prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, - const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, + virtual bool prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, double &x_direction, double &y_direction, double &theta_direction) override; /** @@ -51,7 +51,7 @@ namespace mkt_algorithm * @param traj */ virtual mkt_msgs::Trajectory2D calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new RotateToGoal instance diff --git a/src/Algorithms/Libraries/mkt_algorithm/package.xml b/src/Algorithms/Libraries/mkt_algorithm/package.xml index 7e9bcac..34c48a4 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/package.xml +++ b/src/Algorithms/Libraries/mkt_algorithm/package.xml @@ -51,10 +51,10 @@ catkin geometry_msgs score_algorithm - nav_2d_msgs + robot_nav_2d_msgs roscpp sensor_msgs - nav_2d_utils + robot_nav_2d_utils kalman ddynamic_reconfigure costmap_2d @@ -62,10 +62,10 @@ geometry_msgs score_algorithm - nav_2d_msgs + robot_nav_2d_msgs roscpp sensor_msgs - nav_2d_utils + robot_nav_2d_utils kalman ddynamic_reconfigure costmap_2d @@ -73,10 +73,10 @@ geometry_msgs score_algorithm - nav_2d_msgs + robot_nav_2d_msgs roscpp sensor_msgs - nav_2d_utils + robot_nav_2d_utils kalman ddynamic_reconfigure costmap_2d diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp index 9f88ab2..f2de62e 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -13,8 +13,8 @@ void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFLi tf_ = tf; costmap_robot_ = costmap_robot; - steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); - steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); + steering_fix_wheel_distance_x_ = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); } void GoStraight::reset() @@ -22,8 +22,8 @@ void GoStraight::reset() time_last_cmd_ = robot::Time::now(); } -bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, +bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) { goal_ = goal; @@ -37,9 +37,9 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs:: return true; } -nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) { - nav_2d_msgs::Twist2DStamped result; + robot_nav_2d_msgs::Twist2DStamped result; result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp index 59ba24d..4db5271 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp @@ -1,10 +1,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -26,12 +26,12 @@ void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListen nh_priv_.param("index_samples", index_samples_, 0); nh_priv_.param("filter", is_filter_, false); - steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1); - steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.); - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1); - acc_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1); - decel_lim_theta_ = nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1); - controller_frequency_param_name_ = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + steering_fix_wheel_distance_x_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "steering_fix_wheel_distance_y", 0.); + xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "xy_goal_tolerance", 0.1); + acc_lim_theta_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "acc_lim_theta", 0.1); + decel_lim_theta_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "decel_lim_theta", 0.1); + controller_frequency_param_name_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); if(controller_frequency_param_name_ <= 0.0) { ROS_WARN("controller_frequency is not setting lower 0.0 so will set to 1.0"); @@ -87,8 +87,8 @@ void Bicycle::reset() last_actuator_update_ = robot::Time(0); } -bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, +bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) { nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); @@ -119,9 +119,9 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twi return true; } -nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) { - nav_2d_msgs::Twist2DStamped result; + robot_nav_2d_msgs::Twist2DStamped result; result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; @@ -253,8 +253,8 @@ unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, cons return goal_index; } -bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_msgs::Twist2D& velocity, - const nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, +bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity, + const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal, double &target_direction) { const nav_core2::Costmap &costmap = *costmap_; @@ -265,7 +265,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty."); return false; } - // Chuyển đổi dữ liệu kế hoạch đường đi 'nav_2d_msgs::Path2D' sang 'std::vector' + // Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector' std::vector plan = global_plan.poses; std::vector index_s; @@ -300,7 +300,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ for(int i = 1; i < index_s.size(); ++i) { // double tolerance = line_generator->calculateTolerance(robot_pose, plan[index_s[i-1]]); - double distance = fabs(nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]])); + double distance = fabs(robot_nav_2d_utils::poseDistance(robot_pose, plan[index_s[i-1]])); // double dx = plan[index_s[i-1]].x - robot_pose.x; // double dy = plan[index_s[i-1]].y - robot_pose.y; // if(std::abs(std::sqrt(dx * dx + dy * dy)) <= xy_goal_tolerance_ + 0.15) @@ -331,7 +331,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ } else { - double distance = fabs(nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose)); + double distance = fabs(robot_nav_2d_utils::poseDistance(plan[sub_goal_index], robot_pose)); if(distance <= xy_local_goal_tolerance_ ) { sub_goal_index = (unsigned int)plan.size(); @@ -358,7 +358,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ // && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION) // { // Still on the costmap. Continue. - double distance = fabs(nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét + double distance = fabs(robot_nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét if (distance_to_start > distance) { start_index = i; @@ -431,9 +431,9 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ // std::stringstream ss; for (auto &reached_intermediate_goal : reached_intermediate_goals_) { - double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index])); + double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index])); // ss << distance << " "; - geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(reached_intermediate_goal); + geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal); poseArrayMsg.poses.push_back(pose); } reached_intermediate_goals_pub_.publish(poseArrayMsg); @@ -444,7 +444,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ for (auto &reached_intermediate_goal : reached_intermediate_goals_) { - double distance = fabs(nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index])); + double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index])); if (distance < xy_local_goal_tolerance_) { goal_already_reached = true; @@ -452,7 +452,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ // (start_index is now > goal_index) for (start_index = goal_index; start_index <= last_valid_index; ++start_index) { - distance = nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]); + distance = robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[start_index]); if ( distance >= xy_local_goal_tolerance_offset_) { break; @@ -500,7 +500,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ if (!goal_already_reached) { // new goal not in reached_intermediate_goals_ - double distance = fabs(nav_2d_utils::poseDistance(plan[goal_index], plan[start_index])); + double distance = fabs(robot_nav_2d_utils::poseDistance(plan[goal_index], plan[start_index])); if (distance < xy_local_goal_tolerance_) { // ROS_INFO("goal_index %d distance %f", goal_index, distance); @@ -543,7 +543,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ for(index = start_index+1; index < goal_index; index++) { geometry_msgs::Pose2D pose = plan[index]; - if(nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_) break; + if(robot_nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_) break; } const geometry_msgs::Pose2D p2 = plan[index]; if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9) @@ -569,11 +569,11 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const nav_2d_ } // Publish start_index - geometry_msgs::PoseStamped pose_st = nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now()); + geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now()); closet_robot_goal_pub_.publish(pose_st); // Publish goal_index - geometry_msgs::PoseStamped pose_g = nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now()); + geometry_msgs::PoseStamped pose_g = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now()); current_goal_pub_.publish(pose_g); return true; @@ -605,7 +605,7 @@ bool Bicycle::findSubGoalIndex(const std::vector &plan, s if(target_direction * x_direction_saved < 0.0) { index_s.push_back(i); - geometry_msgs::Pose pose = nav_2d_utils::pose2DToPose(plan[i]); + geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]); poseArrayMsg.poses.push_back(pose); } x_direction_saved = target_direction; @@ -629,7 +629,7 @@ double Bicycle::journey(const std::vector &plan, const un { for(int i= start_index; i < last_valid_index; i++) { - S += nav_2d_utils::poseDistance(plan[i], plan[i+1]); + S += robot_nav_2d_utils::poseDistance(plan[i], plan[i+1]); } } return S; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp index 1f8fccd..c817b83 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -13,8 +13,8 @@ void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TF tf_ = tf; costmap_robot_ = costmap_robot; - steering_fix_wheel_distance_x_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); - steering_fix_wheel_distance_y_ = nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); + steering_fix_wheel_distance_x_ = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_x", 1.1); + steering_fix_wheel_distance_y_ = robot_nav_2d_utils::searchAndGetParam(nh_, "steering_fix_wheel_distance_y", 0.); nh_.param("velocity_rotate_min", velocity_rotate_min_, 0.1); nh_.param("velocity_rotate_max", velocity_rotate_max_, 0.6); nh_.param("angle_pass_rotate", angle_threshold_, 0.02); @@ -25,8 +25,8 @@ void RotateToGoal::reset() time_last_cmd_ = robot::Time::now(); } -bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& velocity, - const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan, +bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity, + const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan, double& x_direction, double& y_direction, double& theta_direction) { goal_ = goal; @@ -38,9 +38,9 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const nav_2d_msgs return true; } -nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D velocity) +robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) { - nav_2d_msgs::Twist2DStamped result; + robot_nav_2d_msgs::Twist2DStamped result; result.header.stamp = robot::Time::now(); geometry_msgs::Pose2D steer_to_baselink_pose; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index 6e07711..01e2f4a 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -85,7 +85,7 @@ void mkt_algorithm::diff::GoStraight::initialize( } mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { mkt_msgs::Trajectory2D result; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; @@ -101,10 +101,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; - nav_2d_msgs::Twist2D drive_cmd; + robot_nav_2d_msgs::Twist2D drive_cmd; double sign_x = x_direction_; - nav_2d_msgs::Twist2D twist; + 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()) @@ -115,7 +115,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( } drive_cmd.x = sqrt(twist.x * twist.x); - nav_2d_msgs::Path2D transformed_plan = transform_plan_; + robot_nav_2d_msgs::Path2D transformed_plan = transform_plan_; if (transformed_plan.poses.empty()) { robot::log_warning("Transformed plan is empty. Cannot determine a local plan."); @@ -152,8 +152,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator( if (std::hypot(carrot_pose.pose.x, carrot_pose.pose.y) > min_journey_squared_) { - nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); - nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; double dx = end.pose.x - start.pose.x; double dy = end.pose.y - start.pose.y; drive_cmd.theta = atan2(dy, dx); 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 d23d651..88a4d12 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 @@ -138,7 +138,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams() "it should be >0. Disabling cost regulated linear velocity scaling."); use_cost_regulated_linear_velocity_scaling_ = false; } - double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); control_duration_ = 1.0 / control_frequency; window_size_ = (size_t)(control_frequency * 3.0); @@ -173,7 +173,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::reset() this->follow_step_path_ = false; this->nav_stop_ = false; last_actuator_update_ = robot::Time::now(); - prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); if (kf_) { @@ -195,7 +195,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::resume() { if(!this->nav_stop_) return; - prevous_drive_cmd_ = nav_2d_msgs::Twist2D(); + prevous_drive_cmd_ = robot_nav_2d_msgs::Twist2D(); last_actuator_update_ = robot::Time::now(); if (kf_) @@ -208,8 +208,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::resume() } } -bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, - const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, +bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, double &x_direction, double &y_direction, double &theta_direction) { if (!initialized_) @@ -313,7 +313,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--) { geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose; - if (nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) + if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_) break; } const geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose; @@ -345,10 +345,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 } geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution() - ? nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) - : nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D()); + ? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose) + : robot_nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D()); - geometry_msgs::Pose back = nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); + geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose); // teb_local_planner::PoseSE2 start_pose(front); // teb_local_planner::PoseSE2 goal_pose(back); @@ -372,7 +372,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2 } mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { mkt_msgs::Trajectory2D result; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; @@ -388,9 +388,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( double dt = (current_time - last_actuator_update_).toSec(); last_actuator_update_ = current_time; - nav_2d_msgs::Twist2D drive_cmd; + robot_nav_2d_msgs::Twist2D drive_cmd; double sign_x = sign(x_direction_); - nav_2d_msgs::Twist2D twist; + robot_nav_2d_msgs::Twist2D twist; traj_->startNewIteration(velocity); while (traj_->hasMoreTwists()) { @@ -399,7 +399,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5); double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x; - nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; + robot_nav_2d_msgs::Path2D transformed_plan = this->transform_plan_; if (transformed_plan.poses.empty()) { robot::log_warning("[%s:%d]\n Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__); @@ -481,7 +481,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } if (fabs(v_theta) > LIMIT_VEL_THETA) { - nav_2d_msgs::Twist2D cmd_vel, cmd_result; + robot_nav_2d_msgs::Twist2D cmd_vel, cmd_result; cmd_vel.x = sign_x > 0 ? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1)) : std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1)); @@ -495,8 +495,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( { if (transform_plan_.poses.size() > 2) { - nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); - nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; + robot_nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back(); + robot_nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2]; double dx = end.pose.x - start.pose.x; double dy = end.pose.y - start.pose.y; v_theta = atan2(dy, dx); @@ -566,7 +566,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator( } bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( - const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &carrot_pose, const nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) + const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, const robot_nav_2d_msgs::Twist2D &velocity, double &angle_to_path, const double &sign_x) { bool curvature = false; double path_angle = 0; @@ -632,7 +632,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath( return result; } -void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel) +void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &angle_to_path, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) { const double dt = control_duration_; const double theta_direction = angle_to_path > 0.0 ? 1.0 : -1.0; @@ -690,7 +690,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::rotateToHeading(const double &an cmd_vel.theta = v_theta_samp; } -double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const nav_2d_msgs::Twist2D &velocity) +double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const robot_nav_2d_msgs::Twist2D &velocity) { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the static look ahead distance @@ -704,8 +704,8 @@ double mkt_algorithm::diff::PredictiveTrajectory::getLookAheadDistance(const nav return lookahead_dist; } -std::vector::iterator -mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan) +std::vector::iterator +mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, robot_nav_2d_msgs::Path2D global_plan) { if (global_plan.poses.empty()) { @@ -768,7 +768,7 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const nav_2d_msgs:: return goal_pose_it; } -geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const nav_2d_msgs::Pose2DStamped &carrot_pose) +geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose) { geometry_msgs::PointStamped carrot_msg; carrot_msg.header = carrot_pose.header; @@ -778,7 +778,7 @@ geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCar return carrot_msg; } -bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) +bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf, const robot_nav_2d_msgs::Pose2DStamped &pose, robot_nav_2d_msgs::Path2D &global_plan, double dist_behind_robot) { if (global_plan.poses.empty()) return false; @@ -787,8 +787,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf try { // let's get the pose of the robot in the frame of the plan - nav_2d_msgs::Pose2DStamped robot; - if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) + robot_nav_2d_msgs::Pose2DStamped robot; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot)) { throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); } @@ -796,8 +796,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf double dist_thresh_sq = dist_behind_robot * dist_behind_robot; // iterate plan until a pose close the robot is found - std::vector::iterator it = global_plan.poses.begin(); - std::vector::iterator erase_end = it; + std::vector::iterator it = global_plan.poses.begin(); + std::vector::iterator erase_end = it; while (it != global_plan.poses.end()) { double dx = robot.pose.x - it->pose.x; @@ -825,9 +825,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf } bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( - TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length, - nav_2d_msgs::Path2D &transformed_plan) + robot_nav_2d_msgs::Path2D &transformed_plan) { // this method is a slightly modified version of base_local_planner/goal_functions.h transformed_plan.poses.clear(); @@ -852,8 +852,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( } // let's get the pose of the robot in the frame of the plan - nav_2d_msgs::Pose2DStamped robot_pose; - if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + robot_nav_2d_msgs::Pose2DStamped robot_pose; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) { throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); } @@ -896,12 +896,12 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( while (i < (int)global_plan.poses.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length)) { - nav_2d_msgs::Pose2DStamped stamped_pose; + robot_nav_2d_msgs::Pose2DStamped stamped_pose; stamped_pose.pose = global_plan.poses[i].pose; stamped_pose.header.frame_id = global_plan.header.frame_id; - nav_2d_msgs::Pose2DStamped newer_pose; - if (!nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) + robot_nav_2d_msgs::Pose2DStamped newer_pose; + if (!robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, newer_pose)) { ++i; continue; @@ -954,7 +954,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan( } void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( - const nav_2d_msgs::Twist2D &velocity, const nav_2d_msgs::Twist2D &cmd_vel, nav_2d_msgs::Twist2D &cmd_vel_result) + const robot_nav_2d_msgs::Twist2D &velocity, const robot_nav_2d_msgs::Twist2D &cmd_vel, robot_nav_2d_msgs::Twist2D &cmd_vel_result) { const double dt = control_duration_; @@ -982,7 +982,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::moveWithAccLimits( // cmd_vel_result.theta = vth; } -bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, nav_2d_msgs::Twist2D &cmd_vel) +bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, robot_nav_2d_msgs::Twist2D &cmd_vel) { // slow down with the maximum possible acceleration... we should really use the frequency that we're running at to determine what is feasible // but we'll use a tenth of a second to be consistent with the implementation of the local planner. @@ -1002,7 +1002,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_m void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( const double &dist_error, const double &lookahead_dist, - const double &curvature, const nav_2d_msgs::Twist2D &velocity, + const double &curvature, const robot_nav_2d_msgs::Twist2D &velocity, const double &pose_cost, double &linear_vel, double &sign_x) { double curvature_vel = linear_vel; @@ -1019,7 +1019,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( if (radius < min_rad) { curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); - nav_2d_msgs::Twist2D cmd, result; + robot_nav_2d_msgs::Twist2D cmd, result; cmd.x = curvature_vel; this->moveWithAccLimits(velocity, cmd, result); curvature_vel = result.x; @@ -1041,7 +1041,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( { cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; } - nav_2d_msgs::Twist2D cmd, result; + robot_nav_2d_msgs::Twist2D cmd, result; cmd.x = cost_vel; this->moveWithAccLimits(velocity, cmd, result); cost_vel = result.x; @@ -1074,7 +1074,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints( } // Use the lowest velocity between approach and other constraints, if all overlapping - nav_2d_msgs::Twist2D cmd, result; + robot_nav_2d_msgs::Twist2D cmd, result; cmd.x = approach_vel; this->moveWithAccLimits(velocity, cmd, result); approach_vel = result.x; @@ -1152,15 +1152,15 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co } void mkt_algorithm::diff::PredictiveTrajectory::updateCostAtOffset( - TFListenerPtr tf, const std::string &robot_base_frame, const nav_2d_msgs::Pose2DStamped &base_pose, + TFListenerPtr tf, const std::string &robot_base_frame, const robot_nav_2d_msgs::Pose2DStamped &base_pose, double x_offset, double y_offset, double &cost_left, double &cost_right) { - nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; + robot_nav_2d_msgs::Pose2DStamped stamped_pose, transformed_pose; stamped_pose = base_pose; stamped_pose.pose.x += x_offset; stamped_pose.pose.y += y_offset; - if (nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) + if (robot_nav_2d_utils::transformPose(tf, robot_base_frame, stamped_pose, transformed_pose)) { double cost = this->costAtPose(stamped_pose.pose.x, stamped_pose.pose.y); if (transformed_pose.pose.y < 0) @@ -1182,7 +1182,7 @@ double mkt_algorithm::diff::PredictiveTrajectory::computeDecelerationFactor(doub return 0.5 * (1.0 + cos(M_PI * (1.0 - ratio))); // Smooth from 1 to 0 } -double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const nav_2d_msgs::Pose2DStamped &carrot_pose, +double mkt_algorithm::diff::PredictiveTrajectory::getEffectiveDistance(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose, double journey_plan) { double carrot_distance = sqrt(carrot_pose.pose.x * carrot_pose.pose.x + @@ -1217,7 +1217,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v return fabs(amplitude) > amplitude_threshold && min_angle * max_angle < 0; } -void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose2DStamped pose) +void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(robot_nav_2d_msgs::Pose2DStamped pose) { const auto &plan_back_pose = compute_plan_.poses.back(); // const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index c624de7..c42683f 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include @@ -24,7 +24,7 @@ void mkt_algorithm::diff::RotateToGoal::initialize( void mkt_algorithm::diff::RotateToGoal::getParams() { - robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); nh_priv_.param("avoid_obstacles", avoid_obstacles_, false); nh_priv_.param("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5); nh_priv_.param("angle_threshold", angle_threshold_, M_PI / 8); @@ -61,7 +61,7 @@ void mkt_algorithm::diff::RotateToGoal::getParams() robot::log_warning("[%s:%d]\n The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__); use_cost_regulated_linear_velocity_scaling_ = false; } - double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); + double control_frequency = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10); control_duration_ = 1.0 / control_frequency; if (traj_) { @@ -87,8 +87,8 @@ void mkt_algorithm::diff::RotateToGoal::reset() this->clear(); } -bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity, - const nav_2d_msgs::Pose2DStamped &goal, const nav_2d_msgs::Path2D &global_plan, +bool mkt_algorithm::diff::RotateToGoal::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity, + const robot_nav_2d_msgs::Pose2DStamped &goal, const robot_nav_2d_msgs::Path2D &global_plan, double &x_direction, double &y_direction, double &theta_direction) { @@ -108,10 +108,10 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped } mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator( - const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) + const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { mkt_msgs::Trajectory2D result; - nav_2d_msgs::Twist2D drive_cmd; + robot_nav_2d_msgs::Twist2D drive_cmd; result.velocity.x = result.velocity.y = result.velocity.theta = 0.0; if (!traj_) return result; diff --git a/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt index 0ed0625..99896dc 100644 --- a/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt @@ -19,7 +19,7 @@ include_directories( # Tạo INTERFACE library (header-only) add_library(mkt_msgs INTERFACE) -target_link_libraries(mkt_msgs INTERFACE nav_2d_msgs geometry_msgs) +target_link_libraries(mkt_msgs INTERFACE robot_nav_2d_msgs geometry_msgs) # Set include directories target_include_directories(mkt_msgs diff --git a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h index 22ef701..ee3d74c 100644 --- a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h +++ b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include namespace mkt_msgs @@ -28,7 +28,7 @@ namespace mkt_msgs (void)_alloc; } - typedef ::nav_2d_msgs::Twist2D_ _velocity_type; + typedef ::robot_nav_2d_msgs::Twist2D_ _velocity_type; _velocity_type velocity; typedef std::vector::template rebind_alloc> _poses_type; diff --git a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt index 55f90aa..f54b834 100644 --- a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt @@ -29,8 +29,8 @@ include_directories( # Dependencies packages (internal libraries) set(PACKAGES_DIR score_algorithm - nav_2d_msgs - nav_2d_utils + robot_nav_2d_msgs + robot_nav_2d_utils nav_core2 geometry_msgs nav_msgs diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h index eb26c63..76bccae 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h @@ -28,8 +28,8 @@ namespace mkt_plugins * @param velocity The robot's current velocity * @return True if goal is reached */ - bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, - const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override; + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &query_pose, const robot_nav_2d_msgs::Pose2DStamped &goal_pose, + const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Factory function to create a GoalChecker instance diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h index 03154f3..99ee362 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h @@ -27,7 +27,7 @@ public: * * Overrides the base class to apply acceleration limits based on acceleration_time. */ - void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) override; + void startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity) override; /** * @brief Factory function to create a LimitedAccelGenerator instance diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h index f1a5973..439660b 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h @@ -46,8 +46,8 @@ namespace mkt_plugins * If stateful is true, once xy position matches, it will not check xy again. * This prevents oscillation when the robot is close to the goal. */ - bool isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, - const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) override; + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &query_pose, const robot_nav_2d_msgs::Pose2DStamped &goal_pose, + const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Factory function to create a SimpleGoalChecker instance diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h index e0f1059..99e88c8 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -68,7 +68,7 @@ namespace mkt_plugins * Resets the velocity iterator and prepares it to generate velocities * reachable from the current velocity. */ - void startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) override; + void startNewIteration(const robot_nav_2d_msgs::Twist2D ¤t_velocity) override; /** * @brief Check if there are more velocity samples available @@ -80,7 +80,7 @@ namespace mkt_plugins * @brief Get the next velocity sample * @return Next valid velocity combination */ - nav_2d_msgs::Twist2D nextTwist() override; + robot_nav_2d_msgs::Twist2D nextTwist() override; /** * @brief Generate a trajectory from start pose to goal @@ -95,10 +95,10 @@ namespace mkt_plugins * 2. Projecting poses forward using kinematic model * 3. Sampling velocities using the velocity iterator */ - mkt_msgs::Trajectory2D generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, - const nav_2d_msgs::Path2D &transformed_plan, - const nav_2d_msgs::Twist2D &start_vel, - const nav_2d_msgs::Twist2D &cmd_vel) override; + mkt_msgs::Trajectory2D generateTrajectory(const robot_nav_2d_msgs::Pose2DStamped &start_pose, + const robot_nav_2d_msgs::Path2D &transformed_plan, + const robot_nav_2d_msgs::Twist2D &start_vel, + const robot_nav_2d_msgs::Twist2D &cmd_vel) override; /** * @brief Get the NodeHandle used for kinematics configuration @@ -126,8 +126,8 @@ namespace mkt_plugins * @param dt amount of time in seconds * @return new velocity after dt seconds */ - virtual nav_2d_msgs::Twist2D computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel, - const nav_2d_msgs::Twist2D &start_vel, + virtual robot_nav_2d_msgs::Twist2D computeNewVelocity(const robot_nav_2d_msgs::Twist2D &cmd_vel, + const robot_nav_2d_msgs::Twist2D &start_vel, const double dt); /** @@ -139,7 +139,7 @@ namespace mkt_plugins * @return New pose after dt seconds */ virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose, - const nav_2d_msgs::Twist2D &vel, + const robot_nav_2d_msgs::Twist2D &vel, const double dt); /** @@ -154,7 +154,7 @@ namespace mkt_plugins * Right now the vector contains a single value repeated many times, but this method could be overridden * to allow for dynamic spacing */ - virtual std::vector getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel); + virtual std::vector getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel); robot::NodeHandle nh_kinematics_; KinematicParameters::Ptr kinematics_; diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h index 6b8d82f..25bbceb 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h @@ -2,7 +2,7 @@ #define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ #include -#include +#include #include namespace mkt_plugins @@ -38,7 +38,7 @@ public: * Resets the iterator and prepares it to generate velocities reachable * from the current velocity within dt seconds. */ - virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) = 0; + virtual void startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) = 0; /** * @brief Check if there are more velocity samples available @@ -53,7 +53,7 @@ public: * Returns the next velocity sample that satisfies kinematic constraints. * Should only be called when hasMoreTwists() returns true. */ - virtual nav_2d_msgs::Twist2D nextTwist() = 0; + virtual robot_nav_2d_msgs::Twist2D nextTwist() = 0; }; // class VelocityIterator } // namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h index 2416b17..fd77c60 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h @@ -38,7 +38,7 @@ namespace mkt_plugins * Creates new OneDVelocityIterator instances for X, Y, and Theta dimensions * based on the current velocity and kinematic constraints. */ - void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) override; + void startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) override; /** * @brief Check if there are more velocity samples to iterate @@ -53,7 +53,7 @@ namespace mkt_plugins * Returns the next valid velocity that satisfies kinematic constraints. * Automatically iterates to the next valid velocity if current is invalid. */ - nav_2d_msgs::Twist2D nextTwist() override; + robot_nav_2d_msgs::Twist2D nextTwist() override; protected: /** diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index 91fa33e..c987633 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -1,9 +1,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -23,11 +23,11 @@ void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh) { if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) { - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); } if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) { - yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); } // ROS_INFO_THROTTLE(0.5, "%s", nh_.getNamespace().c_str()); // ROS_INFO_THROTTLE(0.5, "[GoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f",xy_goal_tolerance_, yaw_goal_tolerance_); @@ -38,8 +38,8 @@ void mkt_plugins::GoalChecker::reset() old_xy_goal_tolerance_ = 0; } -bool mkt_plugins::GoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped &query_pose, const nav_2d_msgs::Pose2DStamped &goal_pose, - const nav_2d_msgs::Path2D &path, const nav_2d_msgs::Twist2D &velocity) +bool mkt_plugins::GoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &query_pose, const robot_nav_2d_msgs::Pose2DStamped &goal_pose, + const robot_nav_2d_msgs::Path2D &path, const robot_nav_2d_msgs::Twist2D &velocity) { this->intParam(nh_); double dx = query_pose.pose.x - goal_pose.pose.x; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp index 29d3638..84ba7ac 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/kinematic_parameters.cpp @@ -1,11 +1,11 @@ #include -#include +#include #include #include #include -using nav_2d_utils::moveDeprecatedParameter; +using robot_nav_2d_utils::moveDeprecatedParameter; namespace mkt_plugins { diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp index ac38e00..6bcb4b9 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include #include @@ -17,7 +17,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) } else { - double controller_frequency = nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0); + double controller_frequency = robot_nav_2d_utils::searchAndGetParam(nh, "controller_frequency", 20.0); if (controller_frequency > 0) { acceleration_time_ = 1.0 / controller_frequency; @@ -31,7 +31,7 @@ void LimitedAccelGenerator::initialize(const robot::NodeHandle& nh) } } -void LimitedAccelGenerator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) +void LimitedAccelGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity) { // Limit our search space to just those within the limited acceleration_time velocity_iterator_->startNewIteration(current_velocity, acceleration_time_); diff --git a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp index 4363f0c..c08fe66 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/simple_goal_checker.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include namespace mkt_plugins @@ -23,15 +23,15 @@ void SimpleGoalChecker::intParam(const robot::NodeHandle& nh) { if(!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) { - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); } if(!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) { - yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); } if(nh.param("stateful", stateful_, true)) { - stateful_ = nav_2d_utils::searchAndGetParam(nh, "stateful", true); + stateful_ = robot_nav_2d_utils::searchAndGetParam(nh, "stateful", true); } robot::log_info_throttle(1.0,"[SimpleGoalChecker] xy_goal_tolerance %f yaw_goal_tolerance %f stateful %x check_xy_ %x" , xy_goal_tolerance_, yaw_goal_tolerance_, stateful_, check_xy_); xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_; @@ -42,8 +42,8 @@ void SimpleGoalChecker::reset() check_xy_ = true; } -bool SimpleGoalChecker::isGoalReached(const nav_2d_msgs::Pose2DStamped& query_pose, const nav_2d_msgs::Pose2DStamped& goal_pose, - const nav_2d_msgs::Path2D& path, const nav_2d_msgs::Twist2D& velocity) +bool SimpleGoalChecker::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& query_pose, const robot_nav_2d_msgs::Pose2DStamped& goal_pose, + const robot_nav_2d_msgs::Path2D& path, const robot_nav_2d_msgs::Twist2D& velocity) { this->intParam(nh_); if (check_xy_) diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp index 0af5486..7c7918b 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include #include @@ -9,7 +9,7 @@ #include #include -using nav_2d_utils::loadParameterWithDeprecation; +using robot_nav_2d_utils::loadParameterWithDeprecation; namespace mkt_plugins { @@ -151,7 +151,7 @@ namespace mkt_plugins return angular; } - void StandardTrajectoryGenerator::startNewIteration(const nav_2d_msgs::Twist2D ¤t_velocity) + void StandardTrajectoryGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D ¤t_velocity) { velocity_iterator_->startNewIteration(current_velocity, sim_time_); } @@ -161,12 +161,12 @@ namespace mkt_plugins return velocity_iterator_->hasMoreTwists(); } - nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist() + robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::nextTwist() { return velocity_iterator_->nextTwist(); } - std::vector StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel) + std::vector StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel) { std::vector steps; if (discretize_by_time_) @@ -196,17 +196,17 @@ namespace mkt_plugins return steps; } - mkt_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const nav_2d_msgs::Pose2DStamped &start_pose, - const nav_2d_msgs::Path2D &transformed_plan, - const nav_2d_msgs::Twist2D &start_vel, - const nav_2d_msgs::Twist2D &cmd_vel) + mkt_msgs::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(const robot_nav_2d_msgs::Pose2DStamped &start_pose, + const robot_nav_2d_msgs::Path2D &transformed_plan, + const robot_nav_2d_msgs::Twist2D &start_vel, + const robot_nav_2d_msgs::Twist2D &cmd_vel) { mkt_msgs::Trajectory2D traj; traj.velocity = cmd_vel; // simulate the trajectory geometry_msgs::Pose2D pose = start_pose.pose; - nav_2d_msgs::Twist2D vel = start_vel; + robot_nav_2d_msgs::Twist2D vel = start_vel; double running_time = 0.0; std::vector steps = getTimeSteps(cmd_vel); @@ -234,10 +234,10 @@ namespace mkt_plugins /** * change vel using acceleration limits to converge towards sample_target-vel */ - nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const nav_2d_msgs::Twist2D &cmd_vel, - const nav_2d_msgs::Twist2D &start_vel, const double dt) + robot_nav_2d_msgs::Twist2D StandardTrajectoryGenerator::computeNewVelocity(const robot_nav_2d_msgs::Twist2D &cmd_vel, + const robot_nav_2d_msgs::Twist2D &start_vel, const double dt) { - nav_2d_msgs::Twist2D new_vel; + robot_nav_2d_msgs::Twist2D new_vel; new_vel.x = projectVelocity(start_vel.x, kinematics_->getAccX(), kinematics_->getDecelX(), dt, cmd_vel.x); new_vel.y = projectVelocity(start_vel.y, kinematics_->getAccY(), kinematics_->getDecelY(), dt, cmd_vel.y); new_vel.theta = projectVelocity(start_vel.theta, kinematics_->getAccTheta(), kinematics_->getDecelTheta(), @@ -246,7 +246,7 @@ namespace mkt_plugins } geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose, - const nav_2d_msgs::Twist2D &vel, const double dt) + const robot_nav_2d_msgs::Twist2D &vel, const double dt) { geometry_msgs::Pose2D new_pose; new_pose.x = start_pose.x + (vel.x * cos(start_pose.theta) + vel.y * cos(M_PI_2 + start_pose.theta)) * dt; diff --git a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp index 9bec193..2820fad 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/xy_theta_iterator.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include namespace mkt_plugins @@ -9,10 +9,10 @@ void XYThetaIterator::initialize(const robot::NodeHandle& nh, KinematicParameter kinematics_ = kinematics; nh.param("vx_samples", vx_samples_, 20); nh.param("vy_samples", vy_samples_, 5); - vtheta_samples_ = nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20); + vtheta_samples_ = robot_nav_2d_utils::loadParameterWithDeprecation(nh, "vtheta_samples", "vth_samples", 20); } -void XYThetaIterator::startNewIteration(const nav_2d_msgs::Twist2D& current_velocity, double dt) +void XYThetaIterator::startNewIteration(const robot_nav_2d_msgs::Twist2D& current_velocity, double dt) { x_it_ = std::make_shared(current_velocity.x, kinematics_->getDirect()[0], kinematics_->getMinX(), kinematics_->getMaxX(), kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); @@ -34,9 +34,9 @@ bool XYThetaIterator::hasMoreTwists() return x_it_ && !x_it_->isFinished(); } -nav_2d_msgs::Twist2D XYThetaIterator::nextTwist() +robot_nav_2d_msgs::Twist2D XYThetaIterator::nextTwist() { - nav_2d_msgs::Twist2D velocity; + robot_nav_2d_msgs::Twist2D velocity; velocity.x = x_it_->getVelocity(); velocity.y = y_it_->getVelocity(); velocity.theta = th_it_->getVelocity(); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt index e7c472e..91cbe0b 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt @@ -33,7 +33,7 @@ set(PACKAGES_DIR std_msgs sensor_msgs visualization_msgs - nav_2d_utils + robot_nav_2d_utils nav_core2 mkt_msgs score_algorithm diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h index 1f33379..54564a3 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h @@ -42,8 +42,8 @@ namespace pnkx_local_planner * @param velocity Current robot velocity * @return The best command for the robot to drive */ - virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. @@ -55,7 +55,7 @@ namespace pnkx_local_planner * @param velocity Current velocity * @return True if the robot should be considered as having reached the goal. */ - virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new instance of the PNKXDockingLocalPlanner @@ -90,12 +90,12 @@ namespace pnkx_local_planner * * Runs the prepare method on all the critics with freshly transformed data */ - virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + virtual void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Iterate through all the twists and find the best one */ - virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + virtual robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; private: class DockingPlanner @@ -113,12 +113,12 @@ namespace pnkx_local_planner void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, score_algorithm::TrajectoryGenerator::Ptr traj_generator); - bool getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal); + bool getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal); - bool geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal); + bool geLocalGoal(robot_nav_2d_msgs::Pose2DStamped &local_goal); - bool getLocalPath(const nav_2d_msgs::Pose2DStamped &local_pose, - const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path); + bool getLocalPath(const robot_nav_2d_msgs::Pose2DStamped &local_pose, + const robot_nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path); bool initialized_; bool is_detected_; @@ -172,7 +172,7 @@ namespace pnkx_local_planner XmlRpc::XmlRpcValue original_papams_; std::vector dkpl_; - bool dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + bool dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity); }; // PNKXDockingLocalPlanner diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h index 1bc39ab..4188c1b 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h @@ -40,8 +40,8 @@ namespace pnkx_local_planner * @param velocity Current robot velocity * @return The best command for the robot to drive */ - nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) override; + robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. @@ -53,7 +53,7 @@ namespace pnkx_local_planner * @param velocity Current velocity * @return True if the robot should be considered as having reached the goal. */ - bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new instance of the PNKXGoStraightLocalPlanner @@ -72,12 +72,12 @@ namespace pnkx_local_planner * * Runs the prepare method on all the critics with freshly transformed data */ - void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Iterate through all the twists and find the best one */ - nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; bool is_ready_; }; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h index e9fb0cc..4e6a3cd 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h @@ -39,13 +39,13 @@ namespace pnkx_local_planner * @brief nav_core2 setGoalPose - Sets the global goal pose * @param goal_pose The Goal Pose */ - void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) override; + void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) override; /** * @brief nav_core2 setPlan - Sets the global plan * @param path The global plan */ - void setPlan(const nav_2d_msgs::Path2D &path) override; + void setPlan(const robot_nav_2d_msgs::Path2D &path) override; /** * @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity @@ -59,8 +59,8 @@ namespace pnkx_local_planner * @param velocity Current robot velocity * @return The best command for the robot to drive */ - nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) override; + robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief set velocity for x, y asix of the robot @@ -108,7 +108,7 @@ namespace pnkx_local_planner * @param velocity Current velocity * @return True if the robot should be considered as having reached the goal. */ - bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new PNKXLocalPlanner @@ -151,17 +151,17 @@ namespace pnkx_local_planner * * Runs the prepare method on all the critics with freshly transformed data */ - virtual void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + virtual void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity); /** * @brief Iterate through all the twists and find the best one */ - virtual nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity); + virtual robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity); /** * @brief Helper method to transform a given pose to the local costmap frame. */ - nav_2d_msgs::Pose2DStamped transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose); + robot_nav_2d_msgs::Pose2DStamped transformPoseToLocal(const robot_nav_2d_msgs::Pose2DStamped &pose); // Plugin handling std::function traj_gen_loader_; @@ -179,9 +179,9 @@ namespace pnkx_local_planner TFListenerPtr tf_; std::string name_; robot::NodeHandle parent_, planner_nh_; - nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan - nav_2d_msgs::Path2D transformed_plan_; - nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose + robot_nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan + robot_nav_2d_msgs::Path2D transformed_plan_; + robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose costmap_2d::Costmap2D *costmap_; costmap_2d::Costmap2DROBOT *costmap_robot_; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h index 400b025..b15388d 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h @@ -40,8 +40,8 @@ namespace pnkx_local_planner * @param velocity Current robot velocity * @return The best command for the robot to drive */ - nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) override; + robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity. @@ -53,7 +53,7 @@ namespace pnkx_local_planner * @param velocity Current velocity * @return True if the robot should be considered as having reached the goal. */ - bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Create a new instance of the PNKXRotateLocalPlanner @@ -72,12 +72,12 @@ namespace pnkx_local_planner * * Runs the prepare method on all the critics with freshly transformed data */ - void prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + void prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; /** * @brief Iterate through all the twists and find the best one */ - nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override; + robot_nav_2d_msgs::Twist2DStamped ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) override; }; } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h index d2ec41a..269fd13 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h @@ -2,8 +2,8 @@ #define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__ #include -#include -#include +#include +#include #include #include @@ -18,10 +18,10 @@ namespace pnkx_local_planner { bool getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, - nav_2d_msgs::Pose2DStamped &global_pose, double transform_tolerance = 2.0); + robot_nav_2d_msgs::Pose2DStamped &global_pose, double transform_tolerance = 2.0); bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame, - const nav_2d_msgs::Pose2DStamped &stamped_pose, nav_2d_msgs::Pose2DStamped &newer_pose); + const robot_nav_2d_msgs::Pose2DStamped &stamped_pose, robot_nav_2d_msgs::Pose2DStamped &newer_pose); double getSquareDistance(const geometry_msgs::Pose2D &pose_a, const geometry_msgs::Pose2D &pose_b); @@ -41,9 +41,9 @@ namespace pnkx_local_planner * @return \c true if the global plan is transformed, \c false otherwise */ bool transformGlobalPlan( - TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose, + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose, const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length, - nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false); + robot_nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false); } 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 ff4eeac..19354ab 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 @@ -1,10 +1,10 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include @@ -224,11 +224,11 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::getParams(robot::NodeHandle &n nh.param("update_costmap_before_planning", update_costmap_before_planning_, true); if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) { - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); } if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) { - yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); } } @@ -249,7 +249,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::reset() nh_algorithm.setParam("allow_rotate", false); } -void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { this->getParams(planner_nh_); if (update_costmap_before_planning_) @@ -267,7 +267,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pos // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), local_goal_pose = this->transformPoseToLocal(goal_pose_); if (start_docking_) { @@ -321,7 +321,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pos if (dkpl_.front()->following_) { - nav_2d_msgs::Pose2DStamped follow_pose; + robot_nav_2d_msgs::Pose2DStamped follow_pose; if (dkpl_.front()->geLocalGoal(local_goal_pose)) { local_goal_pose = follow_pose; @@ -353,11 +353,11 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const nav_2d_msgs::Pos traj_generator_->setDirect(xytheta_direct); } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; try { if (global_plan_.poses.empty()) @@ -373,11 +373,11 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::compute } } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { - nav_2d_msgs::Twist2D twist; + robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; cmd_vel.header.stamp = robot::Time::now(); if (ret_nav_ && ret_angle_) @@ -401,7 +401,7 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::ScoreAl return cmd_vel; } -bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { if (goal_pose_.header.frame_id == "") { @@ -413,16 +413,16 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const nav_2d_msg // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); - nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); - nav_2d_msgs::Path2D plan = transformed_plan_; + 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_; if (start_docking_) { local_goal = goal_pose_; if (!dkpl_.empty() && dkpl_.front()->initialized_) { if (dkpl_.front()->allow_rotate_) - plan = nav_2d_msgs::Path2D(); + plan = robot_nav_2d_msgs::Path2D(); } } @@ -466,7 +466,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::isGoalReached(const nav_2d_msg return ret_nav_ && ret_angle_ && dock_ok; } -bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { if (!dkpl_.empty()) { @@ -483,7 +483,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msg { if (dkpl_.front()->docking_planner_ && !dkpl_.front()->docking_nav_) { - nav_2d_msgs::Pose2DStamped local_goal; + robot_nav_2d_msgs::Pose2DStamped local_goal; try { if (dkpl_.front()->geLocalGoal(local_goal)) @@ -491,9 +491,9 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msg dkpl_.front()->is_detected_ = true; start_docking_ = true; nav_msgs::Path path; - nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); dkpl_.front()->getLocalPath(local_pose, local_goal, path); - this->setPlan(nav_2d_utils::pathToPath(path)); + this->setPlan(robot_nav_2d_utils::pathToPath(path)); this->setGoalPose(local_goal); } } @@ -504,15 +504,15 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const nav_2d_msg } else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_) { - nav_2d_msgs::Pose2DStamped local_goal; + robot_nav_2d_msgs::Pose2DStamped local_goal; try { if (dkpl_.front()->geLocalGoal(local_goal)) { dkpl_.front()->is_detected_ = true; start_docking_ = true; - nav_2d_msgs::Path2D path; - nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); + robot_nav_2d_msgs::Path2D path; + robot_nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); path.header.stamp = robot::Time::now(); path.header.frame_id = costmap_robot_->getGlobalFrameID(); path.poses.push_back(local_pose); @@ -628,7 +628,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob } } - robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); + robot_base_frame_ = robot_nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link")); if (!nh_priv_.param("maker_goal_frame", maker_goal_frame_, std::string(""))) { std::stringstream re; @@ -667,7 +667,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob initialized_ = true; } -bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(nav_2d_msgs::Pose2DStamped &maker_goal) +bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal) { if (!delayed_) return false; @@ -697,17 +697,17 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(n return false; } -bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(nav_2d_msgs::Pose2DStamped &local_goal) +bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(robot_nav_2d_msgs::Pose2DStamped &local_goal) { if (!delayed_) return false; bool get_pose_result = false; try { - nav_2d_msgs::Pose2DStamped maker_goal_on_robot; + robot_nav_2d_msgs::Pose2DStamped maker_goal_on_robot; if (pnkx_local_planner::getPose(tf_, maker_goal_frame_, robot_base_frame_, maker_goal_on_robot, 2.0)) { - if (nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), maker_goal_on_robot, local_goal)) + if (robot_nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), maker_goal_on_robot, local_goal)) { get_pose_result = true; detected_timeout_wt_.stop(); @@ -731,10 +731,10 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(na } bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath( - const nav_2d_msgs::Pose2DStamped &local_pose, const nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path) + const robot_nav_2d_msgs::Pose2DStamped &local_pose, const robot_nav_2d_msgs::Pose2DStamped &local_goal, nav_msgs::Path &local_path) { - geometry_msgs::PoseStamped start = nav_2d_utils::pose2DToPoseStamped(local_pose); - geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(local_goal); + geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose); + geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal); std::vector docking_plan; if (!docking_planner_->makePlan(start, goal, docking_plan)) @@ -744,7 +744,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath( } else { - local_path = nav_2d_utils::posesToPath(docking_plan); + local_path = robot_nav_2d_utils::posesToPath(docking_plan); return true; } } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index c2f1362..0f188bb 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp @@ -1,8 +1,8 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include // #include @@ -139,11 +139,11 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::reset() ret_nav_ = ret_angle_ = false; } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, - const nav_2d_msgs::Twist2D& velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped& pose, + const robot_nav_2d_msgs::Twist2D& velocity) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; try { if (global_plan_.poses.empty()) @@ -158,7 +158,7 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::comp } } -void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { this->getParams(planner_nh_); if (update_costmap_before_planning_) @@ -175,7 +175,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const nav_2d_msgs:: } // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), local_goal_pose = this->transformPoseToLocal(goal_pose_); try @@ -201,11 +201,11 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const nav_2d_msgs:: traj_generator_->setDirect(xytheta_direct); } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { - nav_2d_msgs::Twist2D twist; + robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; if (!ret_nav_) traj = nav_algorithm_->calculator(pose, velocity); @@ -213,7 +213,7 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner::Scor return cmd_vel; } -bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { if (goal_pose_.header.frame_id == "") { 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 d131227..2b81784 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 @@ -1,10 +1,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -169,11 +169,11 @@ void pnkx_local_planner::PNKXLocalPlanner::getParams(robot::NodeHandle &nh) nh.param("update_costmap_before_planning", update_costmap_before_planning_, true); if (!nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.1)) { - xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); + xy_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "xy_goal_tolerance", 0.1); } if (!nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.1)) { - yaw_goal_tolerance_ = nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); + yaw_goal_tolerance_ = robot_nav_2d_utils::searchAndGetParam(nh, "yaw_goal_tolerance", 0.1); } } @@ -188,14 +188,14 @@ void pnkx_local_planner::PNKXLocalPlanner::reset() ret_nav_ = ret_angle_ = false; } -void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) +void pnkx_local_planner::PNKXLocalPlanner::setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); reset(); goal_pose_ = goal_pose; } -void pnkx_local_planner::PNKXLocalPlanner::setPlan(const nav_2d_msgs::Path2D &path) +void pnkx_local_planner::PNKXLocalPlanner::setPlan(const robot_nav_2d_msgs::Path2D &path) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); costmap_robot_->resetLayers(); @@ -208,7 +208,7 @@ void pnkx_local_planner::PNKXLocalPlanner::setPlan(const nav_2d_msgs::Path2D &pa } } -void pnkx_local_planner::PNKXLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { this->getParams(planner_nh_); if (update_costmap_before_planning_) @@ -226,7 +226,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const nav_2d_msgs::Pose2DStam // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + 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_)) @@ -261,11 +261,11 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const nav_2d_msgs::Pose2DStam traj_generator_->setDirect(xytheta_direct); } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose, - const nav_2d_msgs::Twist2D &velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped &pose, + const robot_nav_2d_msgs::Twist2D &velocity) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; try { if (global_plan_.poses.empty()) @@ -281,11 +281,11 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeVelocit } } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { - nav_2d_msgs::Twist2D twist; + robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; cmd_vel.header.stamp = robot::Time::now(); if (ret_nav_ && ret_angle_) @@ -391,7 +391,7 @@ void pnkx_local_planner::PNKXLocalPlanner::unlock() lock_ = false; } -bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) +bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) { if (goal_pose_.header.frame_id == "") { @@ -400,9 +400,9 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const nav_2d_msgs::Pose } // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_pose = this->transformPoseToLocal(pose); - nav_2d_msgs::Pose2DStamped local_goal = this->transformPoseToLocal(goal_pose_); - nav_2d_msgs::Path2D plan = transformed_plan_; + 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_; if (!ret_nav_) { ret_nav_ = goal_checker_->isGoalReached(local_pose, local_goal, plan, velocity); @@ -420,17 +420,17 @@ bool pnkx_local_planner::PNKXLocalPlanner::isGoalReached(const nav_2d_msgs::Pose return ret_nav_ && ret_angle_; } -inline double getSquareDistance(const nav_2d_msgs::Pose2DStamped &pose_a, const nav_2d_msgs::Pose2DStamped &pose_b) +inline double getSquareDistance(const robot_nav_2d_msgs::Pose2DStamped &pose_a, const robot_nav_2d_msgs::Pose2DStamped &pose_b) { double x_diff = pose_a.pose.x - pose_b.pose.x; double y_diff = pose_a.pose.y - pose_b.pose.y; return x_diff * x_diff + y_diff * y_diff; } -nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose) +robot_nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transformPoseToLocal(const robot_nav_2d_msgs::Pose2DStamped &pose) { - nav_2d_msgs::Pose2DStamped local_pose; - nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), pose, local_pose); + robot_nav_2d_msgs::Pose2DStamped local_pose; + robot_nav_2d_utils::transformPose(tf_, costmap_robot_->getGlobalFrameID(), pose, local_pose); return local_pose; } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index 99af078..942d1c2 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -2,10 +2,10 @@ #include "pnkx_local_planner/transforms.h" #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -132,7 +132,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::reset() ret_nav_ = false; } -void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { this->getParams(planner_nh_); if (update_costmap_before_planning_) @@ -150,7 +150,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const nav_2d_msgs::Pose // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), + robot_nav_2d_msgs::Pose2DStamped local_start_pose = this->transformPoseToLocal(pose), local_goal_pose = this->transformPoseToLocal(goal_pose_); try @@ -176,10 +176,10 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const nav_2d_msgs::Pose traj_generator_->setDirect(xytheta_direct); } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { // boost::recursive_mutex::scoped_lock l(configuration_mutex_); - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; try { if (global_plan_.poses.empty()) @@ -194,11 +194,11 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::computeV } } -nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlgorithm(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlgorithm(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { - nav_2d_msgs::Twist2D twist; + robot_nav_2d_msgs::Twist2D twist; mkt_msgs::Trajectory2D traj; - nav_2d_msgs::Twist2DStamped cmd_vel; + robot_nav_2d_msgs::Twist2DStamped cmd_vel; if (ret_nav_) return cmd_vel; @@ -207,7 +207,7 @@ nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::ScoreAlg return cmd_vel; } -bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) +bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const robot_nav_2d_msgs::Pose2DStamped& pose, const robot_nav_2d_msgs::Twist2D& velocity) { if (goal_pose_.header.frame_id == "") { @@ -216,7 +216,7 @@ bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const nav_2d_msgs } // Update time stamp of goal pose // goal_pose_.header.stamp = pose.header.stamp; - nav_2d_msgs::Path2D empty; + robot_nav_2d_msgs::Path2D empty; ret_nav_ = goal_checker_->isGoalReached( this->transformPoseToLocal(pose), this->transformPoseToLocal(goal_pose_), empty, velocity); 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 7b53255..067237f 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 @@ -1,11 +1,11 @@ #include -#include -#include +#include +#include #include #include #include -bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance) +bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, robot_nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance) { if (!tf) return false; @@ -40,7 +40,7 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st tf3::TransformStampedMsg transform = tf->lookupTransform(global_frame, base_frame_id, tf3_zero_time); tf3::doTransform(robot_pose, global_pose_stamped, transform); } - global_pose = nav_2d_utils::poseStampedToPose2D(global_pose_stamped); + global_pose = robot_nav_2d_utils::poseStampedToPose2D(global_pose_stamped); } catch (tf3::LookupException& ex) { @@ -71,11 +71,11 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st } bool pnkx_local_planner::transformGlobalPose(TFListenerPtr tf, const std::string& global_frame, - const nav_2d_msgs::Pose2DStamped& stamped_pose, nav_2d_msgs::Pose2DStamped& newer_pose) + const robot_nav_2d_msgs::Pose2DStamped& stamped_pose, robot_nav_2d_msgs::Pose2DStamped& newer_pose) { if (tf == nullptr) return false; - bool result = nav_2d_utils::transformPose(tf, global_frame, stamped_pose, newer_pose); + bool result = robot_nav_2d_utils::transformPose(tf, global_frame, stamped_pose, newer_pose); newer_pose.header.seq = stamped_pose.header.seq; return result; } @@ -88,9 +88,9 @@ double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a } bool pnkx_local_planner::transformGlobalPlan( - TFListenerPtr tf, const nav_2d_msgs::Path2D& global_plan, const nav_2d_msgs::Pose2DStamped& pose, + TFListenerPtr tf, const robot_nav_2d_msgs::Path2D& global_plan, const robot_nav_2d_msgs::Pose2DStamped& pose, const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length, - nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame) + robot_nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame) { if (global_plan.poses.size() == 0) { @@ -108,8 +108,8 @@ bool pnkx_local_planner::transformGlobalPlan( try { // let's get the pose of the robot in the frame of the plan - nav_2d_msgs::Pose2DStamped robot_pose; - if (!nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) + robot_nav_2d_msgs::Pose2DStamped robot_pose; + if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose)) { throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame"); } @@ -117,8 +117,8 @@ bool pnkx_local_planner::transformGlobalPlan( transformed_plan.header.frame_id = costmap->getGlobalFrameID(); transformed_plan.header.stamp = pose.header.stamp; - nav_2d_msgs::Pose2DStamped new_pose; - nav_2d_msgs::Pose2DStamped stamped_pose; + robot_nav_2d_msgs::Pose2DStamped new_pose; + robot_nav_2d_msgs::Pose2DStamped stamped_pose; stamped_pose.header.frame_id = global_plan.header.frame_id; for (unsigned int i = 0; i < global_plan.poses.size(); i++) @@ -136,15 +136,15 @@ bool pnkx_local_planner::transformGlobalPlan( // Otherwise it may take a few iterations to converge to the proper behavior // let's get the pose of the robot in the frame of the transformed_plan/costmap - nav_2d_msgs::Pose2DStamped costmap_pose; - if (!nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose)) + robot_nav_2d_msgs::Pose2DStamped costmap_pose; + if (!robot_nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose)) { throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame"); } robot::log_assert(global_plan.poses.size() >= transformed_plan.poses.size(), "Global plan size is less than transformed plan size"); - std::vector::iterator it = transformed_plan.poses.begin(); + std::vector::iterator it = transformed_plan.poses.begin(); double sq_prune_dist = 0.1; @@ -168,7 +168,7 @@ bool pnkx_local_planner::transformGlobalPlan( // if(from_global_frame) sq_prune_dist = 0; while (it != transformed_plan.poses.end()) { - const nav_2d_msgs::Pose2DStamped& w = *it; + const robot_nav_2d_msgs::Pose2DStamped& w = *it; // 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) { diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h deleted file mode 100644 index b4ca539..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h +++ /dev/null @@ -1,74 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/ComplexPolygon2D.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H -#define NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct ComplexPolygon2D_ -{ - typedef ComplexPolygon2D_ Type; - - ComplexPolygon2D_() - : outer() - , inner() { - } - ComplexPolygon2D_(const ContainerAllocator& _alloc) - : outer(_alloc) - , inner(_alloc) { - (void)_alloc; - } - - - - typedef ::nav_2d_msgs::Polygon2D_ _outer_type; - _outer_type outer; - - typedef std::vector< ::nav_2d_msgs::Polygon2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Polygon2D_ >> _inner_type; - _inner_type inner; - - - - - - typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_ const> ConstPtr; - -}; // struct ComplexPolygon2D_ - -typedef ::nav_2d_msgs::ComplexPolygon2D_ > ComplexPolygon2D; - -typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr; -typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::nav_2d_msgs::ComplexPolygon2D_ & rhs) -{ - return lhs.outer == rhs.outer && - lhs.inner == rhs.inner; -} - -template -bool operator!=(const ::nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::nav_2d_msgs::ComplexPolygon2D_ & rhs) -{ - return !(lhs == rhs); -} - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h deleted file mode 100644 index 9603f07..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h +++ /dev/null @@ -1,80 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/NavGridOfChars.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H -#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct NavGridOfChars_ -{ - typedef NavGridOfChars_ Type; - - NavGridOfChars_() - : stamp() - , info() - , data() { - } - NavGridOfChars_(const ContainerAllocator& _alloc) - : stamp() - , info(_alloc) - , data(_alloc) { - (void)_alloc; - } - - - - typedef robot::Time _stamp_type; - _stamp_type stamp; - - typedef ::nav_2d_msgs::NavGridInfo_ _info_type; - _info_type info; - - typedef std::vector::template rebind_alloc> _data_type; - _data_type data; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_ const> ConstPtr; - -}; // struct NavGridOfChars_ - -typedef ::nav_2d_msgs::NavGridOfChars_ > NavGridOfChars; - -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::NavGridOfChars_ & lhs, const ::nav_2d_msgs::NavGridOfChars_ & rhs) -{ - return lhs.stamp == rhs.stamp && - lhs.info == rhs.info && - lhs.data == rhs.data; -} - -template -bool operator!=(const ::nav_2d_msgs::NavGridOfChars_ & lhs, const ::nav_2d_msgs::NavGridOfChars_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h deleted file mode 100644 index e892439..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h +++ /dev/null @@ -1,80 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/NavGridOfCharsUpdate.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H -#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct NavGridOfCharsUpdate_ -{ - typedef NavGridOfCharsUpdate_ Type; - - NavGridOfCharsUpdate_() - : stamp() - , bounds() - , data() { - } - NavGridOfCharsUpdate_(const ContainerAllocator& _alloc) - : stamp() - , bounds(_alloc) - , data(_alloc) { - (void)_alloc; - } - - - - typedef robot::Time _stamp_type; - _stamp_type stamp; - - typedef ::nav_2d_msgs::UIntBounds_ _bounds_type; - _bounds_type bounds; - - typedef std::vector::template rebind_alloc> _data_type; - _data_type data; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_ const> ConstPtr; - -}; // struct NavGridOfCharsUpdate_ - -typedef ::nav_2d_msgs::NavGridOfCharsUpdate_ > NavGridOfCharsUpdate; - -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) -{ - return lhs.stamp == rhs.stamp && - lhs.bounds == rhs.bounds && - lhs.data == rhs.data; -} - -template -bool operator!=(const ::nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h deleted file mode 100644 index f2829b0..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h +++ /dev/null @@ -1,80 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/NavGridOfDoubles.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H -#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct NavGridOfDoubles_ -{ - typedef NavGridOfDoubles_ Type; - - NavGridOfDoubles_() - : stamp() - , info() - , data() { - } - NavGridOfDoubles_(const ContainerAllocator& _alloc) - : stamp() - , info(_alloc) - , data(_alloc) { - (void)_alloc; - } - - - - typedef robot::Time _stamp_type; - _stamp_type stamp; - - typedef ::nav_2d_msgs::NavGridInfo_ _info_type; - _info_type info; - - typedef std::vector::template rebind_alloc> _data_type; - _data_type data; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_ const> ConstPtr; - -}; // struct NavGridOfDoubles_ - -typedef ::nav_2d_msgs::NavGridOfDoubles_ > NavGridOfDoubles; - -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::nav_2d_msgs::NavGridOfDoubles_ & rhs) -{ - return lhs.stamp == rhs.stamp && - lhs.info == rhs.info && - lhs.data == rhs.data; -} - -template -bool operator!=(const ::nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::nav_2d_msgs::NavGridOfDoubles_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h deleted file mode 100644 index 1706c71..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h +++ /dev/null @@ -1,80 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/NavGridOfDoublesUpdate.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H -#define NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct NavGridOfDoublesUpdate_ -{ - typedef NavGridOfDoublesUpdate_ Type; - - NavGridOfDoublesUpdate_() - : stamp() - , bounds() - , data() { - } - NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc) - : stamp() - , bounds(_alloc) - , data(_alloc) { - (void)_alloc; - } - - - - typedef robot::Time _stamp_type; - _stamp_type stamp; - - typedef ::nav_2d_msgs::UIntBounds_ _bounds_type; - _bounds_type bounds; - - typedef std::vector::template rebind_alloc> _data_type; - _data_type data; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_ const> ConstPtr; - -}; // struct NavGridOfDoublesUpdate_ - -typedef ::nav_2d_msgs::NavGridOfDoublesUpdate_ > NavGridOfDoublesUpdate; - -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) -{ - return lhs.stamp == rhs.stamp && - lhs.bounds == rhs.bounds && - lhs.data == rhs.data; -} - -template -bool operator!=(const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h deleted file mode 100644 index 4f0b967..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h +++ /dev/null @@ -1,74 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Path2D.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_PATH2D_H -#define NAV_2D_MSGS_MESSAGE_PATH2D_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct Path2D_ -{ - typedef Path2D_ Type; - - Path2D_() - : header() - , poses() { - } - Path2D_(const ContainerAllocator& _alloc) - : header() - , poses(_alloc) { - (void)_alloc; - } - - - - typedef ::std_msgs::Header _header_type; - _header_type header; - - typedef std::vector< ::nav_2d_msgs::Pose2DStamped_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Pose2DStamped_ >> _poses_type; - _poses_type poses; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Path2D_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Path2D_ const> ConstPtr; - -}; // struct Path2D_ - -typedef ::nav_2d_msgs::Path2D_ > Path2D; - -typedef std::shared_ptr< ::nav_2d_msgs::Path2D > Path2DPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Path2D const> Path2DConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Path2D_ & lhs, const ::nav_2d_msgs::Path2D_ & rhs) -{ - return lhs.header == rhs.header && - lhs.poses == rhs.poses; -} - -template -bool operator!=(const ::nav_2d_msgs::Path2D_ & lhs, const ::nav_2d_msgs::Path2D_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_PATH2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h deleted file mode 100644 index f192042..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h +++ /dev/null @@ -1,72 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Point2D.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POINT2D_H -#define NAV_2D_MSGS_MESSAGE_POINT2D_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct Point2D_ -{ - typedef Point2D_ Type; - - Point2D_() - : x(0.0) - , y(0.0) { - } - Point2D_(const ContainerAllocator& _alloc) - : x(0.0) - , y(0.0) { - (void)_alloc; - } - - - - typedef double _x_type; - _x_type x; - - typedef double _y_type; - _y_type y; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Point2D_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Point2D_ const> ConstPtr; - -}; // struct Point2D_ - -typedef ::nav_2d_msgs::Point2D_ > Point2D; - -typedef std::shared_ptr< ::nav_2d_msgs::Point2D > Point2DPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Point2D const> Point2DConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Point2D_ & lhs, const ::nav_2d_msgs::Point2D_ & rhs) -{ - return lhs.x == rhs.x && - lhs.y == rhs.y; -} - -template -bool operator!=(const ::nav_2d_msgs::Point2D_ & lhs, const ::nav_2d_msgs::Point2D_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POINT2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h deleted file mode 100644 index 934e145..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h +++ /dev/null @@ -1,67 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Polygon2D.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2D_H -#define NAV_2D_MSGS_MESSAGE_POLYGON2D_H - - -#include -#include -#include - -#include - -namespace nav_2d_msgs -{ -template -struct Polygon2D_ -{ - typedef Polygon2D_ Type; - - Polygon2D_() - : points() { - } - Polygon2D_(const ContainerAllocator& _alloc) - : points(_alloc) { - (void)_alloc; - } - - - - typedef std::vector< ::nav_2d_msgs::Point2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::Point2D_ >> _points_type; - _points_type points; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_ const> ConstPtr; - -}; // struct Polygon2D_ - -typedef ::nav_2d_msgs::Polygon2D_ > Polygon2D; - -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D > Polygon2DPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D const> Polygon2DConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Polygon2D_ & lhs, const ::nav_2d_msgs::Polygon2D_ & rhs) -{ - return lhs.points == rhs.points; -} - -template -bool operator!=(const ::nav_2d_msgs::Polygon2D_ & lhs, const ::nav_2d_msgs::Polygon2D_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POLYGON2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h deleted file mode 100644 index c528e9f..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h +++ /dev/null @@ -1,81 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Polygon2DCollection.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H -#define NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H - - -#include -#include -#include - -#include -#include -#include - -namespace nav_2d_msgs -{ -template -struct Polygon2DCollection_ -{ - typedef Polygon2DCollection_ Type; - - Polygon2DCollection_() - : header() - , polygons() - , colors() { - } - Polygon2DCollection_(const ContainerAllocator& _alloc) - : header() - , polygons(_alloc) - , colors(_alloc) { - (void)_alloc; - } - - - - typedef ::std_msgs::Header _header_type; - _header_type header; - - typedef std::vector< ::nav_2d_msgs::ComplexPolygon2D_ , typename std::allocator_traits::template rebind_alloc< ::nav_2d_msgs::ComplexPolygon2D_ >> _polygons_type; - _polygons_type polygons; - - typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type; - _colors_type colors; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_ const> ConstPtr; - -}; // struct Polygon2DCollection_ - -typedef ::nav_2d_msgs::Polygon2DCollection_ > Polygon2DCollection; - -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Polygon2DCollection_ & lhs, const ::nav_2d_msgs::Polygon2DCollection_ & rhs) -{ - return lhs.header == rhs.header && - lhs.polygons == rhs.polygons && - lhs.colors == rhs.colors; -} - -template -bool operator!=(const ::nav_2d_msgs::Polygon2DCollection_ & lhs, const ::nav_2d_msgs::Polygon2DCollection_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h deleted file mode 100644 index bb34d54..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h +++ /dev/null @@ -1,74 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Polygon2DStamped.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H -#define NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct Polygon2DStamped_ -{ - typedef Polygon2DStamped_ Type; - - Polygon2DStamped_() - : header() - , polygon() { - } - Polygon2DStamped_(const ContainerAllocator& _alloc) - : header() - , polygon(_alloc) { - (void)_alloc; - } - - - - typedef ::std_msgs::Header _header_type; - _header_type header; - - typedef ::nav_2d_msgs::Polygon2D_ _polygon_type; - _polygon_type polygon; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_ const> ConstPtr; - -}; // struct Polygon2DStamped_ - -typedef ::nav_2d_msgs::Polygon2DStamped_ > Polygon2DStamped; - -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Polygon2DStamped_ & lhs, const ::nav_2d_msgs::Polygon2DStamped_ & rhs) -{ - return lhs.header == rhs.header && - lhs.polygon == rhs.polygon; -} - -template -bool operator!=(const ::nav_2d_msgs::Polygon2DStamped_ & lhs, const ::nav_2d_msgs::Polygon2DStamped_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h deleted file mode 100644 index 367c668..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h +++ /dev/null @@ -1,78 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Pose2D32.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POSE2D32_H -#define NAV_2D_MSGS_MESSAGE_POSE2D32_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct Pose2D32_ -{ - typedef Pose2D32_ Type; - - Pose2D32_() - : x(0.0) - , y(0.0) - , theta(0.0) { - } - Pose2D32_(const ContainerAllocator& _alloc) - : x(0.0) - , y(0.0) - , theta(0.0) { - (void)_alloc; - } - - - - typedef float _x_type; - _x_type x; - - typedef float _y_type; - _y_type y; - - typedef float _theta_type; - _theta_type theta; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_ const> ConstPtr; - -}; // struct Pose2D32_ - -typedef ::nav_2d_msgs::Pose2D32_ > Pose2D32; - -typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 > Pose2D32Ptr; -typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Pose2D32_ & lhs, const ::nav_2d_msgs::Pose2D32_ & rhs) -{ - return lhs.x == rhs.x && - lhs.y == rhs.y && - lhs.theta == rhs.theta; -} - -template -bool operator!=(const ::nav_2d_msgs::Pose2D32_ & lhs, const ::nav_2d_msgs::Pose2D32_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POSE2D32_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h deleted file mode 100644 index f8ebac1..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h +++ /dev/null @@ -1,74 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Pose2DStamped.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H -#define NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct Pose2DStamped_ -{ - typedef Pose2DStamped_ Type; - - Pose2DStamped_() - : header() - , pose() { - } - Pose2DStamped_(const ContainerAllocator& _alloc) - : header() - , pose() { - (void)_alloc; - } - - - - typedef ::std_msgs::Header _header_type; - _header_type header; - - typedef ::geometry_msgs::Pose2D _pose_type; - _pose_type pose; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_ const> ConstPtr; - -}; // struct Pose2DStamped_ - -typedef ::nav_2d_msgs::Pose2DStamped_ > Pose2DStamped; - -typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped > Pose2DStampedPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped const> Pose2DStampedConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Pose2DStamped_ & lhs, const ::nav_2d_msgs::Pose2DStamped_ & rhs) -{ - return lhs.header == rhs.header && - lhs.pose == rhs.pose; -} - -template -bool operator!=(const ::nav_2d_msgs::Pose2DStamped_ & lhs, const ::nav_2d_msgs::Pose2DStamped_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h deleted file mode 100644 index 9d215b3..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h +++ /dev/null @@ -1,30 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/SwitchPlugin.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H -#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H - - -#include -#include - - -namespace nav_2d_msgs -{ - -struct SwitchPlugin -{ - -typedef SwitchPluginRequest Request; -typedef SwitchPluginResponse Response; -Request request; -Response response; - -typedef Request RequestType; -typedef Response ResponseType; - -}; // struct SwitchPlugin -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h deleted file mode 100644 index 82eae9d..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h +++ /dev/null @@ -1,66 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/SwitchPluginRequest.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H -#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct SwitchPluginRequest_ -{ - typedef SwitchPluginRequest_ Type; - - SwitchPluginRequest_() - : new_plugin() { - } - SwitchPluginRequest_(const ContainerAllocator& _alloc) - : new_plugin(_alloc) { - (void)_alloc; - } - - - - typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _new_plugin_type; - _new_plugin_type new_plugin; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_ const> ConstPtr; - -}; // struct SwitchPluginRequest_ - -typedef ::nav_2d_msgs::SwitchPluginRequest_ > SwitchPluginRequest; - -typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest > SwitchPluginRequestPtr; -typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest const> SwitchPluginRequestConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::nav_2d_msgs::SwitchPluginRequest_ & rhs) -{ - return lhs.new_plugin == rhs.new_plugin; -} - -template -bool operator!=(const ::nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::nav_2d_msgs::SwitchPluginRequest_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h deleted file mode 100644 index 295f8de..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h +++ /dev/null @@ -1,72 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/SwitchPluginResponse.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H -#define NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct SwitchPluginResponse_ -{ - typedef SwitchPluginResponse_ Type; - - SwitchPluginResponse_() - : success(false) - , message() { - } - SwitchPluginResponse_(const ContainerAllocator& _alloc) - : success(false) - , message(_alloc) { - (void)_alloc; - } - - - - typedef uint8_t _success_type; - _success_type success; - - typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _message_type; - _message_type message; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_ const> ConstPtr; - -}; // struct SwitchPluginResponse_ - -typedef ::nav_2d_msgs::SwitchPluginResponse_ > SwitchPluginResponse; - -typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse > SwitchPluginResponsePtr; -typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse const> SwitchPluginResponseConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::nav_2d_msgs::SwitchPluginResponse_ & rhs) -{ - return lhs.success == rhs.success && - lhs.message == rhs.message; -} - -template -bool operator!=(const ::nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::nav_2d_msgs::SwitchPluginResponse_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h deleted file mode 100644 index 120c6cc..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h +++ /dev/null @@ -1,78 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Twist2D.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D_H -#define NAV_2D_MSGS_MESSAGE_TWIST2D_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct Twist2D_ -{ - typedef Twist2D_ Type; - - Twist2D_() - : x(0.0) - , y(0.0) - , theta(0.0) { - } - Twist2D_(const ContainerAllocator& _alloc) - : x(0.0) - , y(0.0) - , theta(0.0) { - (void)_alloc; - } - - - - typedef double _x_type; - _x_type x; - - typedef double _y_type; - _y_type y; - - typedef double _theta_type; - _theta_type theta; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_ const> ConstPtr; - -}; // struct Twist2D_ - -typedef ::nav_2d_msgs::Twist2D_ > Twist2D; - -typedef std::shared_ptr< ::nav_2d_msgs::Twist2D > Twist2DPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Twist2D const> Twist2DConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Twist2D_ & lhs, const ::nav_2d_msgs::Twist2D_ & rhs) -{ - return lhs.x == rhs.x && - lhs.y == rhs.y && - lhs.theta == rhs.theta; -} - -template -bool operator!=(const ::nav_2d_msgs::Twist2D_ & lhs, const ::nav_2d_msgs::Twist2D_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_TWIST2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h deleted file mode 100644 index 1c56809..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h +++ /dev/null @@ -1,78 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Twist2D32.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_TWIST2D32_H -#define NAV_2D_MSGS_MESSAGE_TWIST2D32_H - - -#include -#include -#include - - -namespace nav_2d_msgs -{ -template -struct Twist2D32_ -{ - typedef Twist2D32_ Type; - - Twist2D32_() - : x(0.0) - , y(0.0) - , theta(0.0) { - } - Twist2D32_(const ContainerAllocator& _alloc) - : x(0.0) - , y(0.0) - , theta(0.0) { - (void)_alloc; - } - - - - typedef float _x_type; - _x_type x; - - typedef float _y_type; - _y_type y; - - typedef float _theta_type; - _theta_type theta; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_ const> ConstPtr; - -}; // struct Twist2D32_ - -typedef ::nav_2d_msgs::Twist2D32_ > Twist2D32; - -typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 > Twist2D32Ptr; -typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32 const> Twist2D32ConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Twist2D32_ & lhs, const ::nav_2d_msgs::Twist2D32_ & rhs) -{ - return lhs.x == rhs.x && - lhs.y == rhs.y && - lhs.theta == rhs.theta; -} - -template -bool operator!=(const ::nav_2d_msgs::Twist2D32_ & lhs, const ::nav_2d_msgs::Twist2D32_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_TWIST2D32_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h b/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h deleted file mode 100644 index 5cb5620..0000000 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h +++ /dev/null @@ -1,74 +0,0 @@ -// Generated by gencpp from file nav_2d_msgs/Twist2DStamped.msg -// DO NOT EDIT! - - -#ifndef NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H -#define NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H - - -#include -#include -#include - -#include -#include - -namespace nav_2d_msgs -{ -template -struct Twist2DStamped_ -{ - typedef Twist2DStamped_ Type; - - Twist2DStamped_() - : header() - , velocity() { - } - Twist2DStamped_(const ContainerAllocator& _alloc) - : header() - , velocity(_alloc) { - (void)_alloc; - } - - - - typedef ::std_msgs::Header _header_type; - _header_type header; - - typedef ::nav_2d_msgs::Twist2D_ _velocity_type; - _velocity_type velocity; - - - - - typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_ const> ConstPtr; - -}; // struct Twist2DStamped_ - -typedef ::nav_2d_msgs::Twist2DStamped_ > Twist2DStamped; - -typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped > Twist2DStampedPtr; -typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped const> Twist2DStampedConstPtr; - -// constants requiring out of line definition - - - -template -bool operator==(const ::nav_2d_msgs::Twist2DStamped_ & lhs, const ::nav_2d_msgs::Twist2DStamped_ & rhs) -{ - return lhs.header == rhs.header && - lhs.velocity == rhs.velocity; -} - -template -bool operator!=(const ::nav_2d_msgs::Twist2DStamped_ & lhs, const ::nav_2d_msgs::Twist2DStamped_ & rhs) -{ - return !(lhs == rhs); -} - - -} // namespace nav_2d_msgs - -#endif // NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h deleted file mode 100755 index 6c95c26..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_BOUNDS_H -#define NAV_2D_UTILS_BOUNDS_H - -#include -#include -#include - -/** - * @brief A set of utility functions for Bounds objects interacting with other messages/types - */ -namespace nav_2d_utils -{ - -/** - * @brief return a floating point bounds that covers the entire NavGrid - * @param info Info for the NavGrid - * @return bounds covering the entire NavGrid - */ -nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info); - -/** - * @brief return an integral bounds that covers the entire NavGrid - * @param info Info for the NavGrid - * @return bounds covering the entire NavGrid - */ -nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info); - -/** - * @brief Translate real-valued bounds to uint coordinates based on nav_grid info - * @param info Information used when converting the coordinates - * @param bounds floating point bounds object - * @returns corresponding UIntBounds for parameter - */ -nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds); - -/** - * @brief Translate uint bounds to real-valued coordinates based on nav_grid info - * @param info Information used when converting the coordinates - * @param bounds UIntBounds object - * @returns corresponding floating point bounds for parameter - */ -nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds); - -/** - * @brief divide the given bounds up into sub-bounds of roughly equal size - * @param original_bounds The original bounds to divide - * @param n_cols Positive number of columns to divide the bounds into - * @param n_rows Positive number of rows to divide the bounds into - * @return vector of a maximum of n_cols*n_rows nonempty bounds - * @throws std::invalid_argument when n_cols or n_rows is zero - */ -std::vector divideBounds(const nav_core2::UIntBounds& original_bounds, - unsigned int n_cols, unsigned int n_rows); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_BOUNDS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h deleted file mode 100755 index ce0e66f..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h +++ /dev/null @@ -1,106 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_CONVERSIONS_H -#define NAV_2D_UTILS_CONVERSIONS_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// #include -#include -#include - -namespace nav_2d_utils -{ -// Twist Transformations -geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d); -nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel); - -// Point Transformations -nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point); -nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point); -geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point); -geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point); - -// Pose Transformations -// nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose); -nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose); -geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d); -geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d); -geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, - const std::string& frame, const robot::Time& stamp); - -// Path Transformations -nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path); -nav_msgs::Path posesToPath(const std::vector& poses); -nav_2d_msgs::Path2D posesToPath2D(const std::vector& poses); -nav_msgs::Path poses2DToPath(const std::vector& poses, - const std::string& frame, const robot::Time& stamp); -nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d); - -// Polygon Transformations -geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d); -nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d); -geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d); -nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d); - -// Info Transformations -nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info); -nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg); -geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info); -geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info); -nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame); -nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info); - -// Bounds Transformations -nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds); -nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_CONVERSIONS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h deleted file mode 100755 index cab4803..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h +++ /dev/null @@ -1,153 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_PARAMETERS_H -#define NAV_2D_UTILS_PARAMETERS_H - -#include -#include -#include -#include - -namespace nav_2d_utils -{ - -/** - * @brief Search for a parameter and load it, or use the default value - * - * This templated function shortens a commonly used ROS pattern in which you - * search for a parameter and get its value if it exists, otherwise returning a default value. - * - * @param nh NodeHandle to start the parameter search from - * @param param_name Name of the parameter to search for - * @param default_value Value to return if not found - * @return Value of parameter if found, otherwise the default_value - */ -template -param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value) -{ - std::string resolved_name; - if (nh.searchParam(param_name, resolved_name)) - { - param_t value = default_value; - robot::NodeHandle nh_private = robot::NodeHandle("~"); - nh_private.param(resolved_name, value, default_value); - return value; - } - return default_value; -} - -/** - * @brief Load a parameter from one of two namespaces. Complain if it uses the old name. - * @param nh NodeHandle to look for the parameter in - * @param current_name Parameter name that is current, i.e. not deprecated - * @param old_name Deprecated parameter name - * @param default_value If neither parameter is present, return this value - * @return The value of the parameter or the default value - */ -template -param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::string current_name, - const std::string old_name, const param_t& default_value) -{ - param_t value; - if (nh.hasParam(current_name)) - { - nh.getParam(current_name, value, default_value); - return value; - } - if (nh.hasParam(old_name)) - { - robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); - nh.getParam(old_name, value, default_value); - return value; - } - return default_value; -} - -/** - * @brief If a deprecated parameter exists, complain and move to its new location - * @param nh NodeHandle to look for the parameter in - * @param current_name Parameter name that is current, i.e. not deprecated - * @param old_name Deprecated parameter name - */ -template -void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name) -{ - if (!nh.hasParam(old_name)) return; - - param_t value; - robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); - value = nh.param(old_name); - nh.setParam(current_name, value); -} - -/** - * @brief Move a parameter from one place to another - * - * For use loading old parameter structures into new places. - * - * If the new name already has a value, don't move the old value there. - * - * @param nh NodeHandle for loading/saving params - * @param old_name Parameter name to move/remove - * @param current_name Destination parameter name - * @param default_value Value to save in the new name if old parameter is not found. - * @param should_delete If true, whether to delete the parameter from the old name - */ -template -void moveParameter(const robot::NodeHandle& nh, std::string old_name, - std::string current_name, param_t default_value, bool should_delete = true) -{ - // if (nh.hasParam(current_name)) - // { - // if (should_delete) - // nh.deleteParam(old_name); - // return; - // } - // XmlRpc::XmlRpcValue value; - // if (nh.hasParam(old_name)) - // { - // nh.getParam(old_name, value); - // if (should_delete) nh.deleteParam(old_name); - // } - // else - // value = default_value; - - // nh.setParam(current_name, value); -} - - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_PARAMETERS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h deleted file mode 100755 index 187b762..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h +++ /dev/null @@ -1,88 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_PATH_OPS_H -#define NAV_2D_UTILS_PATH_OPS_H - -#include - -namespace nav_2d_utils -{ -/** - * @brief Calculate the linear distance between two poses - */ -double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1); - -/** - * @brief Calculate the length of the plan, starting from the given index - */ -double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index = 0); - -/** - * @brief Calculate the length of the plan from the pose on the plan closest to the given pose - */ -double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose); - -/** - * @brief Increase plan resolution to match that of the costmap by adding points linearly between points - * - * @param global_plan_in input plan - * @param resolution desired distance between waypoints - * @return Higher resolution plan - */ -nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution); - -/** - * @brief Decrease the length of the plan by eliminating colinear points - * - * Uses the Ramer Douglas Peucker algorithm. Ignores theta values - * - * @param input_path Path to compress - * @param epsilon maximum allowable error. Increase for greater compression. - * @return Path2D with possibly fewer poses - */ -nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon = 0.1); - -/** - * @brief Convenience function to add a pose to a path in one line. - * @param path Path to add to - * @param x x-coordinate - * @param y y-coordinate - * @param theta theta (if needed) - */ -void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_PATH_OPS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h deleted file mode 100755 index c9ad2e7..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h +++ /dev/null @@ -1,274 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_PLUGIN_MUX_H -#define NAV_2D_UTILS_PLUGIN_MUX_H - -#include -#include -#include -#include -#include -#include -#include - -namespace nav_2d_utils -{ -/** - * @class PluginMux - * @brief An organizer for switching between multiple different plugins of the same type - * - * The different plugins are specified using a list of strings on the parameter server, each of which is a namespace. - * The specific type and additional parameters for each plugin are specified on the parameter server in that namespace. - * All the plugins are loaded initially, but only one is the "current" plugin at any particular time, which is published - * on a latched topic AND stored on the ROS parameter server. You can switch which plugin is current either through a - * C++ or ROS interface. - */ -template -class PluginMux -{ -public: - /** - * @brief Main Constructor - Loads plugin(s) and sets up ROS interfaces - * - * @param plugin_package The package of the plugin type - * @param plugin_class The class name for the plugin type - * @param parameter_name Name of parameter for the namespaces. - * @param default_value If class name is not specified, which plugin should be loaded - * @param ros_name ROS name for setting up topic and parameter - * @param switch_service_name ROS name for setting up the ROS service - */ - PluginMux(const std::string& plugin_package, const std::string& plugin_class, - const std::string& parameter_name, const std::string& default_value, - const std::string& ros_name = "current_plugin", const std::string& switch_service_name = "switch_plugin"); - - /** - * @brief Create an instance of the given plugin_class_name and save it with the given plugin_name - * @param plugin_name Namespace for the new plugin - * @param plugin_class_name Class type name for the new plugin - */ - void addPlugin(const std::string& plugin_name, const std::string& plugin_class_name); - - /** - * @brief C++ Interface for switching to a given plugin - * - * @param name Namespace to set current plugin to - * @return true if that namespace exists and is loaded properly - */ - bool usePlugin(const std::string& name) - { - // If plugin with given name doesn't exist, return false - if (plugins_.count(name) == 0) return false; - - if (switch_callback_) - { - switch_callback_(current_plugin_, name); - } - - // Switch Mux - current_plugin_ = name; - - // Update ROS info - std_msgs::String str_msg; - str_msg.data = current_plugin_; - current_plugin_pub_.publish(str_msg); - private_nh_.setParam(ros_name_, current_plugin_); - - return true; - } - - /** - * @brief Get the Current Plugin Name - * @return Name of the current plugin - */ - std::string getCurrentPluginName() const - { - return current_plugin_; - } - - /** - * @brief Check to see if a plugin exists - * @param name Namespace to set current plugin to - * @return True if the plugin exists - */ - bool hasPlugin(const std::string& name) const - { - return plugins_.find(name) != plugins_.end(); - } - - /** - * @brief Get the Specified Plugin - * @param name Name of plugin to get - * @return Reference to specified plugin - */ - T& getPlugin(const std::string& name) - { - if (!hasPlugin(name)) - throw std::invalid_argument("Plugin not found"); - return *plugins_[name]; - } - - /** - * @brief Get the Current Plugin - * @return Reference to current plugin - */ - T& getCurrentPlugin() - { - return getPlugin(current_plugin_); - } - - /** - * @brief Get the current list of plugin names - */ - std::vector getPluginNames() const - { - std::vector names; - for (auto& kv : plugins_) - { - names.push_back(kv.first); - } - return names; - } - - /** - * @brief alias for the function-type of the callback fired when the plugin switches. - * - * The first parameter will be the namespace of the plugin that was previously used. - * The second parameter will be the namespace of the plugin that is being switched to. - */ - using SwitchCallback = std::function; - - /** - * @brief Set the callback fired when the plugin switches - * - * In addition to switching which plugin is being used via directly calling `usePlugin` - * a switch can also be triggered externally via ROS service. In such a case, other classes - * may want to know when this happens. This is accomplished using the SwitchCallback, which - * will be called regardless of how the plugin is switched. - */ - void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; } - -protected: - /** - * @brief ROS Interface for Switching Plugins - */ - bool switchPluginService(nav_2d_msgs::SwitchPlugin::Request &req, nav_2d_msgs::SwitchPlugin::Response &resp) - { - std::string name = req.new_plugin; - if (usePlugin(name)) - { - resp.success = true; - resp.message = "Loaded plugin namespace " + name + "."; - } - else - { - resp.success = false; - resp.message = "Namespace " + name + " not configured!"; - } - return true; - } - - // Plugin Management - pluginlib::ClassLoader plugin_loader_; - std::map> plugins_; - std::string current_plugin_; - - // ROS Interface - robot::ServiceServer switch_plugin_srv_; - robot::Publisher current_plugin_pub_; - robot::NodeHandle private_nh_; - std::string ros_name_; - - // Switch Callback - SwitchCallback switch_callback_; -}; - -// ********************************************************************************************************************* -// ****************** Implementation of Templated Methods ************************************************************** -// ********************************************************************************************************************* -template -PluginMux::PluginMux(const std::string& plugin_package, const std::string& plugin_class, - const std::string& parameter_name, const std::string& default_value, - const std::string& ros_name, const std::string& switch_service_name) - : plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr) -{ - // Create the latched publisher - current_plugin_pub_ = private_nh_.advertise(ros_name_, 1, true); - - // Load Plugins - std::string plugin_class_name; - std::vector plugin_namespaces; - private_nh_.getParam(parameter_name, plugin_namespaces); - if (plugin_namespaces.size() == 0) - { - // If no namespaces are listed, use the name of the default class as the singular namespace. - std::string plugin_name = plugin_loader_.getName(default_value); - plugin_namespaces.push_back(plugin_name); - } - - for (const std::string& the_namespace : plugin_namespaces) - { - // Load the class name from namespace/plugin_class, or use default value - private_nh_.param(std::string(the_namespace + "/plugin_class"), plugin_class_name, default_value); - addPlugin(the_namespace, plugin_class_name); - } - - // By default, use the first one as current - usePlugin(plugin_namespaces[0]); - - // Now that the plugins are created, advertise the service if there are multiple - if (plugin_namespaces.size() > 1) - { - switch_plugin_srv_ = private_nh_.advertiseService(switch_service_name, &PluginMux::switchPluginService, this); - } -} - -template -void PluginMux::addPlugin(const std::string& plugin_name, const std::string& plugin_class_name) -{ - try - { - plugins_[plugin_name] = plugin_loader_.createInstance(plugin_class_name); - } - catch (const pluginlib::PluginlibException& ex) - { - ROS_FATAL_NAMED("PluginMux", - "Failed to load the plugin: %s. Exception: %s", plugin_name.c_str(), ex.what()); - exit(EXIT_FAILURE); - } -} - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_PLUGIN_MUX_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h b/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h deleted file mode 100755 index e17581f..0000000 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef NAV_2D_UTILS_TF_HELP_H -#define NAV_2D_UTILS_TF_HELP_H - -#include -#include -#include -#include - -namespace nav_2d_utils -{ -/** - * @brief Transform a PoseStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. - * @return True if successful transform - */ - bool transformPose(const TFListenerPtr tf, const std::string frame, - const geometry_msgs::PoseStamped& in_pose, geometry_msgs::PoseStamped& out_pose, - const bool extrapolation_fallback = true); - -/** - * @brief Transform a Pose2DStamped from one frame to another while catching exceptions - * - * Also returns immediately if the frames are equal. - * @param tf Smart pointer to TFListener - * @param frame Frame to transform the pose into - * @param in_pose Pose to transform - * @param out_pose Place to store the resulting transformed pose - * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. - * @return True if successful transform - */ -bool transformPose(const TFListenerPtr tf, const std::string frame, - const nav_2d_msgs::Pose2DStamped& in_pose, nav_2d_msgs::Pose2DStamped& out_pose, - const bool extrapolation_fallback = true); - -/** - * @brief Transform a Pose2DStamped into another frame - * - * Note that this returns a transformed pose - * regardless of whether the transform was successfully performed. - * - * @param tf Smart pointer to TFListener - * @param pose Pose to transform - * @param frame_id Frame to transform the pose into - * @return The resulting transformed pose - */ -geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped& pose, - const std::string& frame_id); - -} // namespace nav_2d_utils - -#endif // NAV_2D_UTILS_TF_HELP_H diff --git a/src/Libraries/nav_2d_utils/src/bounds.cpp b/src/Libraries/nav_2d_utils/src/bounds.cpp deleted file mode 100755 index 4a0ea72..0000000 --- a/src/Libraries/nav_2d_utils/src/bounds.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include -#include - -namespace nav_2d_utils -{ -nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo& info) -{ - return nav_core2::Bounds(info.origin_x, info.origin_y, - info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height); -} - -nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo& info) -{ - // bounds are inclusive, so we subtract one - return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1); -} - -nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::Bounds& bounds) -{ - unsigned int g_min_x, g_min_y, g_max_x, g_max_y; - worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y); - worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y); - return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y); -} - -nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo& info, const nav_core2::UIntBounds& bounds) -{ - double min_x, min_y, max_x, max_y; - gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y); - gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y); - return nav_core2::Bounds(min_x, min_y, max_x, max_y); -} - -std::vector divideBounds(const nav_core2::UIntBounds& original_bounds, - unsigned int n_cols, unsigned int n_rows) -{ - if (n_cols * n_rows == 0) - { - throw std::invalid_argument("Number of rows and columns must be positive (not zero)"); - } - unsigned int full_width = original_bounds.getWidth(), - full_height = original_bounds.getHeight(); - - unsigned int small_width = static_cast(ceil(static_cast(full_width) / n_cols)), - small_height = static_cast(ceil(static_cast(full_height) / n_rows)); - - std::vector divided; - - for (unsigned int row = 0; row < n_rows; row++) - { - unsigned int min_y = original_bounds.getMinY() + small_height * row; - unsigned int max_y = std::min(min_y + small_height - 1, original_bounds.getMaxY()); - - for (unsigned int col = 0; col < n_cols; col++) - { - unsigned int min_x = original_bounds.getMinX() + small_width * col; - unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX()); - nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y); - if (!sub.isEmpty()) - divided.push_back(sub); - } - } - return divided; -} -} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/conversions.cpp b/src/Libraries/nav_2d_utils/src/conversions.cpp deleted file mode 100755 index 4d50d9a..0000000 --- a/src/Libraries/nav_2d_utils/src/conversions.cpp +++ /dev/null @@ -1,339 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#include -#include -#include - -namespace nav_2d_utils -{ - -geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D& cmd_vel_2d) -{ - geometry_msgs::Twist cmd_vel; - cmd_vel.linear.x = cmd_vel_2d.x; - cmd_vel.linear.y = cmd_vel_2d.y; - cmd_vel.angular.z = cmd_vel_2d.theta; - return cmd_vel; -} - - -nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist& cmd_vel) -{ - nav_2d_msgs::Twist2D cmd_vel_2d; - cmd_vel_2d.x = cmd_vel.linear.x; - cmd_vel_2d.y = cmd_vel.linear.y; - cmd_vel_2d.theta = cmd_vel.angular.z; - return cmd_vel_2d; -} - -nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point& point) -{ - nav_2d_msgs::Point2D output; - output.x = point.x; - output.y = point.y; - return output; -} - -nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32& point) -{ - nav_2d_msgs::Point2D output; - output.x = point.x; - output.y = point.y; - return output; -} - -geometry_msgs::Point pointToPoint3D(const nav_2d_msgs::Point2D& point) -{ - geometry_msgs::Point output; - output.x = point.x; - output.y = point.y; - return output; -} - -geometry_msgs::Point32 pointToPoint32(const nav_2d_msgs::Point2D& point) -{ - geometry_msgs::Point32 output; - output.x = point.x; - output.y = point.y; - return output; -} - -// nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose) -// { -// nav_2d_msgs::Pose2DStamped pose2d; -// pose2d.header.stamp = pose.stamp_; -// pose2d.header.frame_id = pose.frame_id_; -// pose2d.pose.x = pose.getOrigin().getX(); -// pose2d.pose.y = pose.getOrigin().getY(); -// pose2d.pose.theta = tf::getYaw(pose.getRotation()); -// return pose2d; -// } - -nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped& pose) -{ - nav_2d_msgs::Pose2DStamped pose2d; - 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); - return pose2d; -} - -geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D& pose2d) -{ - geometry_msgs::Pose pose; - pose.position.x = pose2d.x; - pose.position.y = pose2d.y; - // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); - return pose; -} - -geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped& pose2d) -{ - geometry_msgs::PoseStamped pose; - pose.header = pose2d.header; - pose.pose = pose2DToPose(pose2d.pose); - return pose; -} - -// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, -// const std::string& frame, const robot::Time& stamp) -// { -// geometry_msgs::PoseStamped pose; -// pose.header.frame_id = frame; -// pose.header.stamp = stamp; -// pose.pose.position.x = pose2d.x; -// pose.pose.position.y = pose2d.y; -// // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); -// return pose; -// } - -nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path& path) -{ - nav_2d_msgs::Path2D path2d; - path2d.header = path.header; - nav_2d_msgs::Pose2DStamped stamped_2d; - path2d.poses.resize(path.poses.size()); - for (unsigned int i = 0; i < path.poses.size(); i++) - { - stamped_2d = poseStampedToPose2D(path.poses[i]); - path2d.poses[i] = stamped_2d; - } - return path2d; -} - -nav_msgs::Path posesToPath(const std::vector& poses) -{ - nav_msgs::Path path; - if (poses.empty()) - return path; - - path.poses.resize(poses.size()); - path.header.frame_id = poses[0].header.frame_id; - path.header.stamp = poses[0].header.stamp; - for (unsigned int i = 0; i < poses.size(); i++) - { - path.poses[i] = poses[i]; - } - return path; -} - -nav_2d_msgs::Path2D posesToPath2D(const std::vector& poses) -{ - nav_2d_msgs::Path2D path; - if (poses.empty()) - return path; - - nav_2d_msgs::Pose2DStamped pose; - path.poses.resize(poses.size()); - path.header.frame_id = poses[0].header.frame_id; - path.header.stamp = poses[0].header.stamp; - for (unsigned int i = 0; i < poses.size(); i++) - { - pose = poseStampedToPose2D(poses[i]); - path.poses[i] = pose; - } - return path; -} - - -// nav_msgs::Path poses2DToPath(const std::vector& poses, -// const std::string& frame, const robot::Time& stamp) -// { -// nav_msgs::Path path; -// path.poses.resize(poses.size()); -// path.header.frame_id = frame; -// path.header.stamp = stamp; -// for (unsigned int i = 0; i < poses.size(); i++) -// { -// path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp); -// } -// return path; -// } - -nav_msgs::Path pathToPath(const nav_2d_msgs::Path2D& path2d) -{ - nav_msgs::Path path; - path.header = path2d.header; - path.poses.resize(path2d.poses.size()); - for (unsigned int i = 0; i < path.poses.size(); i++) - { - path.poses[i].header = path2d.header; - path.poses[i].pose = pose2DToPose(path2d.poses[i].pose); - } - return path; -} - -geometry_msgs::Polygon polygon2Dto3D(const nav_2d_msgs::Polygon2D& polygon_2d) -{ - geometry_msgs::Polygon polygon; - polygon.points.reserve(polygon_2d.points.size()); - for (const auto& pt : polygon_2d.points) - { - polygon.points.push_back(pointToPoint32(pt)); - } - return polygon; -} - -nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon& polygon_3d) -{ - nav_2d_msgs::Polygon2D polygon; - polygon.points.reserve(polygon_3d.points.size()); - for (const auto& pt : polygon_3d.points) - { - polygon.points.push_back(pointToPoint2D(pt)); - } - return polygon; -} - -geometry_msgs::PolygonStamped polygon2Dto3D(const nav_2d_msgs::Polygon2DStamped& polygon_2d) -{ - geometry_msgs::PolygonStamped polygon; - polygon.header = polygon_2d.header; - polygon.polygon = polygon2Dto3D(polygon_2d.polygon); - return polygon; -} - -nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped& polygon_3d) -{ - nav_2d_msgs::Polygon2DStamped polygon; - polygon.header = polygon_3d.header; - polygon.polygon = polygon3Dto2D(polygon_3d.polygon); - return polygon; -} - -nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo& info) -{ - nav_2d_msgs::NavGridInfo msg; - msg.width = info.width; - msg.height = info.height; - msg.resolution = info.resolution; - msg.frame_id = info.frame_id; - msg.origin_x = info.origin_x; - msg.origin_y = info.origin_y; - return msg; -} - -nav_grid::NavGridInfo fromMsg(const nav_2d_msgs::NavGridInfo& msg) -{ - nav_grid::NavGridInfo info; - info.width = msg.width; - info.height = msg.height; - info.resolution = msg.resolution; - info.frame_id = msg.frame_id; - info.origin_x = msg.origin_x; - info.origin_y = msg.origin_y; - return info; -} - -nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData& metadata, const std::string& frame) -{ - nav_grid::NavGridInfo info; - info.frame_id = frame; - info.resolution = metadata.resolution; - info.width = metadata.width; - 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; - // } - return info; -} - -geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo& info) -{ - geometry_msgs::Pose origin; - origin.position.x = info.origin_x; - origin.position.y = info.origin_y; - origin.orientation.w = 1.0; - return origin; -} - -geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo& info) -{ - geometry_msgs::Pose2D origin; - origin.x = info.origin_x; - origin.y = info.origin_y; - return origin; -} - -nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo & info) -{ - nav_msgs::MapMetaData metadata; - metadata.resolution = info.resolution; - metadata.width = info.width; - metadata.height = info.height; - metadata.origin = getOrigin3D(info); - return metadata; -} - -nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds& bounds) -{ - nav_2d_msgs::UIntBounds msg; - msg.min_x = bounds.getMinX(); - msg.min_y = bounds.getMinY(); - msg.max_x = bounds.getMaxX(); - msg.max_y = bounds.getMaxY(); - return msg; -} - -nav_core2::UIntBounds fromMsg(const nav_2d_msgs::UIntBounds& msg) -{ - return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y); -} - - -} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/path_ops.cpp b/src/Libraries/nav_2d_utils/src/path_ops.cpp deleted file mode 100755 index a5a026a..0000000 --- a/src/Libraries/nav_2d_utils/src/path_ops.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include -#include -#include -#include - -namespace nav_2d_utils -{ - -double poseDistance(const geometry_msgs::Pose2D& pose0, const geometry_msgs::Pose2D& pose1) -{ - return hypot(pose0.x - pose1.x, pose0.y - pose1.y); -} - -double getPlanLength(const nav_2d_msgs::Path2D& plan, const unsigned int start_index) -{ - double length = 0.0; - for (unsigned int i = start_index + 1; i < plan.poses.size(); i++) - { - length += poseDistance(plan.poses[i - 1].pose, plan.poses[i].pose); - } - return length; -} - -double getPlanLength(const nav_2d_msgs::Path2D& plan, const geometry_msgs::Pose2D& query_pose) -{ - if (plan.poses.size() == 0) return 0.0; - - unsigned int closest_index = 0; - double closest_distance = poseDistance(plan.poses[0].pose, query_pose); - for (unsigned int i = 1; i < plan.poses.size(); i++) - { - double distance = poseDistance(plan.poses[i].pose, query_pose); - if (closest_distance > distance) - { - closest_index = i; - closest_distance = distance; - } - } - return getPlanLength(plan, closest_index); -} - -nav_2d_msgs::Path2D adjustPlanResolution(const nav_2d_msgs::Path2D& global_plan_in, double resolution) -{ - nav_2d_msgs::Path2D global_plan_out; - global_plan_out.header = global_plan_in.header; - if (global_plan_in.poses.size() == 0) - { - return global_plan_out; - } - - nav_2d_msgs::Pose2DStamped last_stp = global_plan_in.poses[0]; - global_plan_out.poses.push_back(last_stp); - - double sq_resolution = resolution * resolution; - geometry_msgs::Pose2D last = last_stp.pose; - for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i) - { - geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose; - double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y); - if (sq_dist > sq_resolution) - { - // add points in-between - double diff = sqrt(sq_dist) - resolution; - double steps_double = ceil(diff / resolution) + 1.0; - int steps = static_cast(steps_double); - - double delta_x = (loop.x - last.x) / steps_double; - double delta_y = (loop.y - last.y) / steps_double; - double delta_t = (loop.theta - last.theta) / steps_double; - - for (int j = 1; j < steps; ++j) - { - nav_2d_msgs::Pose2DStamped pose; - pose.header = last_stp.header; - pose.pose.x = last.x + j * delta_x; - pose.pose.y = last.y + j * delta_y; - pose.pose.theta = last.theta + j * delta_t; - global_plan_out.poses.push_back(pose); - } - } - global_plan_out.poses.push_back(global_plan_in.poses[i]); - last.x = loop.x; - last.y = loop.y; - } - return global_plan_out; -} - -using PoseList = std::vector; - -/** - * @brief Helper function for other version of compressPlan. - * - * Uses the Ramer Douglas Peucker algorithm. Ignores theta values - * - * @param input Full list of poses - * @param start_index Index of first pose (inclusive) - * @param end_index Index of last pose (inclusive) - * @param epsilon maximum allowable error. Increase for greater compression. - * @param list of poses possibly compressed for the poses[start_index, end_index] - */ -PoseList compressPlan(const PoseList& input, unsigned int start_index, unsigned int end_index, double epsilon) -{ - // Skip if only two points - if (end_index - start_index <= 1) - { - PoseList::const_iterator first = input.begin() + start_index; - PoseList::const_iterator last = input.begin() + end_index + 1; - return PoseList(first, last); - } - - // Find the point with the maximum distance to the line from start to end - const nav_2d_msgs::Pose2DStamped& start = input[start_index], - end = input[end_index]; - double max_distance = 0.0; - unsigned int max_index = 0; - for (unsigned int i = start_index + 1; i < end_index; i++) - { - const nav_2d_msgs::Pose2DStamped& pose = input[i]; - double d = distanceToLine(pose.pose.x, pose.pose.y, start.pose.x, start.pose.y, end.pose.x, end.pose.y); - if (d > max_distance) - { - max_index = i; - max_distance = d; - } - } - - // If max distance is less than epsilon, eliminate all the points between start and end - if (max_distance <= epsilon) - { - PoseList outer; - outer.push_back(start); - outer.push_back(end); - return outer; - } - - // If max distance is greater than epsilon, recursively simplify - PoseList first_part = compressPlan(input, start_index, max_index, epsilon); - PoseList second_part = compressPlan(input, max_index, end_index, epsilon); - first_part.insert(first_part.end(), second_part.begin() + 1, second_part.end()); - return first_part; -} - -nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double epsilon) -{ - nav_2d_msgs::Path2D results; - results.header = input_path.header; - results.poses = compressPlan(input_path.poses, 0, input_path.poses.size() - 1, epsilon); - return results; -} - -void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta) -{ - nav_2d_msgs::Pose2DStamped pose; - pose.pose.x = x; - pose.pose.y = y; - pose.pose.theta = theta; - path.poses.push_back(pose); -} -} // namespace nav_2d_utils diff --git a/src/Libraries/nav_2d_msgs/CMakeLists.txt b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt similarity index 71% rename from src/Libraries/nav_2d_msgs/CMakeLists.txt rename to src/Libraries/robot_nav_2d_msgs/CMakeLists.txt index ae2aefd..165063b 100755 --- a/src/Libraries/nav_2d_msgs/CMakeLists.txt +++ b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10) -project(nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX) +project(robot_nav_2d_msgs VERSION 1.0.0 LANGUAGES CXX) # Chuẩn C++ set(CMAKE_CXX_STANDARD 17) @@ -8,13 +8,13 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) # Tìm tất cả header files -file(GLOB_RECURSE HEADERS "include/nav_2d_msgs/*.h") +file(GLOB_RECURSE HEADERS "include/robot_nav_2d_msgs/*.h") # Tạo INTERFACE library (header-only) -add_library(nav_2d_msgs INTERFACE) +add_library(robot_nav_2d_msgs INTERFACE) # Set include directories -target_include_directories(nav_2d_msgs +target_include_directories(robot_nav_2d_msgs INTERFACE $ $ @@ -22,30 +22,30 @@ target_include_directories(nav_2d_msgs # Link dependencies (header-only, chỉ cần include paths) # Các dependencies này cũng là header-only từ common_msgs -target_link_libraries(nav_2d_msgs +target_link_libraries(robot_nav_2d_msgs INTERFACE std_msgs geometry_msgs ) # Cài đặt header files -install(DIRECTORY include/nav_2d_msgs +install(DIRECTORY include/robot_nav_2d_msgs DESTINATION include FILES_MATCHING PATTERN "*.h") # Cài đặt target -install(TARGETS nav_2d_msgs - EXPORT nav_2d_msgs-targets +install(TARGETS robot_nav_2d_msgs + EXPORT robot_nav_2d_msgs-targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) # Export targets # Bây giờ có thể export dependencies vì std_msgs và geometry_msgs đã được export -install(EXPORT nav_2d_msgs-targets - FILE nav_2d_msgs-targets.cmake - NAMESPACE nav_2d_msgs:: - DESTINATION lib/cmake/nav_2d_msgs) +install(EXPORT robot_nav_2d_msgs-targets + FILE robot_nav_2d_msgs-targets.cmake + NAMESPACE robot_nav_2d_msgs:: + DESTINATION lib/cmake/robot_nav_2d_msgs) # In thông tin cấu hình message(STATUS "=================================") diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h new file mode 100644 index 0000000..535bfe4 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file robot_nav_2d_msgs/ComplexPolygon2D.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct ComplexPolygon2D_ +{ + typedef ComplexPolygon2D_ Type; + + ComplexPolygon2D_() + : outer() + , inner() { + } + ComplexPolygon2D_(const ContainerAllocator& _alloc) + : outer(_alloc) + , inner(_alloc) { + (void)_alloc; + } + + + + typedef ::robot_nav_2d_msgs::Polygon2D_ _outer_type; + _outer_type outer; + + typedef std::vector< ::robot_nav_2d_msgs::Polygon2D_ , typename std::allocator_traits::template rebind_alloc< ::robot_nav_2d_msgs::Polygon2D_ >> _inner_type; + _inner_type inner; + + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_ const> ConstPtr; + +}; // struct ComplexPolygon2D_ + +typedef ::robot_nav_2d_msgs::ComplexPolygon2D_ > ComplexPolygon2D; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D > ComplexPolygon2DPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D const> ComplexPolygon2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_ & rhs) +{ + return lhs.outer == rhs.outer && + lhs.inner == rhs.inner; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::ComplexPolygon2D_ & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_ & rhs) +{ + return !(lhs == rhs); +} + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h similarity index 59% rename from src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h index 91adb59..b1da6e2 100644 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h @@ -1,15 +1,15 @@ -// Generated by gencpp from file nav_2d_msgs/NavGridInfo.msg +// Generated by gencpp from file robot_nav_2d_msgs/NavGridInfo.msg // DO NOT EDIT! -#ifndef NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H -#define NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H #include #include #include -namespace nav_2d_msgs +namespace robot_nav_2d_msgs { template struct NavGridInfo_ @@ -57,22 +57,22 @@ struct NavGridInfo_ - typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_ const> ConstPtr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_ const> ConstPtr; }; // struct NavGridInfo_ -typedef ::nav_2d_msgs::NavGridInfo_ > NavGridInfo; +typedef ::robot_nav_2d_msgs::NavGridInfo_ > NavGridInfo; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo > NavGridInfoPtr; -typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo > NavGridInfoPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo const> NavGridInfoConstPtr; // constants requiring out of line definition template -bool operator==(const ::nav_2d_msgs::NavGridInfo_ & lhs, const ::nav_2d_msgs::NavGridInfo_ & rhs) +bool operator==(const ::robot_nav_2d_msgs::NavGridInfo_ & lhs, const ::robot_nav_2d_msgs::NavGridInfo_ & rhs) { return lhs.width == rhs.width && lhs.height == rhs.height && @@ -83,12 +83,12 @@ bool operator==(const ::nav_2d_msgs::NavGridInfo_ & lhs, co } template -bool operator!=(const ::nav_2d_msgs::NavGridInfo_ & lhs, const ::nav_2d_msgs::NavGridInfo_ & rhs) +bool operator!=(const ::robot_nav_2d_msgs::NavGridInfo_ & lhs, const ::robot_nav_2d_msgs::NavGridInfo_ & rhs) { return !(lhs == rhs); } -} // namespace nav_2d_msgs +} // namespace robot_nav_2d_msgs -#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDINFO_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h new file mode 100644 index 0000000..c3ca50e --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfChars.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct NavGridOfChars_ +{ + typedef NavGridOfChars_ Type; + + NavGridOfChars_() + : stamp() + , info() + , data() { + } + NavGridOfChars_(const ContainerAllocator& _alloc) + : stamp() + , info(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::robot_nav_2d_msgs::NavGridInfo_ _info_type; + _info_type info; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_ const> ConstPtr; + +}; // struct NavGridOfChars_ + +typedef ::robot_nav_2d_msgs::NavGridOfChars_ > NavGridOfChars; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars > NavGridOfCharsPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars const> NavGridOfCharsConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::NavGridOfChars_ & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.info == rhs.info && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::NavGridOfChars_ & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h new file mode 100644 index 0000000..d9ade13 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfCharsUpdate.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct NavGridOfCharsUpdate_ +{ + typedef NavGridOfCharsUpdate_ Type; + + NavGridOfCharsUpdate_() + : stamp() + , bounds() + , data() { + } + NavGridOfCharsUpdate_(const ContainerAllocator& _alloc) + : stamp() + , bounds(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::robot_nav_2d_msgs::UIntBounds_ _bounds_type; + _bounds_type bounds; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ const> ConstPtr; + +}; // struct NavGridOfCharsUpdate_ + +typedef ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ > NavGridOfCharsUpdate; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate > NavGridOfCharsUpdatePtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate const> NavGridOfCharsUpdateConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.bounds == rhs.bounds && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h new file mode 100644 index 0000000..e0360a6 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfDoubles.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct NavGridOfDoubles_ +{ + typedef NavGridOfDoubles_ Type; + + NavGridOfDoubles_() + : stamp() + , info() + , data() { + } + NavGridOfDoubles_(const ContainerAllocator& _alloc) + : stamp() + , info(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::robot_nav_2d_msgs::NavGridInfo_ _info_type; + _info_type info; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_ const> ConstPtr; + +}; // struct NavGridOfDoubles_ + +typedef ::robot_nav_2d_msgs::NavGridOfDoubles_ > NavGridOfDoubles; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles > NavGridOfDoublesPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles const> NavGridOfDoublesConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.info == rhs.info && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoubles_ & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h new file mode 100644 index 0000000..2fa99e8 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h @@ -0,0 +1,80 @@ +// Generated by gencpp from file robot_nav_2d_msgs/NavGridOfDoublesUpdate.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct NavGridOfDoublesUpdate_ +{ + typedef NavGridOfDoublesUpdate_ Type; + + NavGridOfDoublesUpdate_() + : stamp() + , bounds() + , data() { + } + NavGridOfDoublesUpdate_(const ContainerAllocator& _alloc) + : stamp() + , bounds(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef ::robot_nav_2d_msgs::UIntBounds_ _bounds_type; + _bounds_type bounds; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ const> ConstPtr; + +}; // struct NavGridOfDoublesUpdate_ + +typedef ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ > NavGridOfDoublesUpdate; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate > NavGridOfDoublesUpdatePtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate const> NavGridOfDoublesUpdateConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) +{ + return lhs.stamp == rhs.stamp && + lhs.bounds == rhs.bounds && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h new file mode 100644 index 0000000..08684a4 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h @@ -0,0 +1,69 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Path2D.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct Path2D_ +{ + typedef Path2D_ Type; + + Path2D_() + : header() + , poses() { + } + Path2D_(const ContainerAllocator& _alloc) + : header() + , poses(_alloc) { + (void)_alloc; + } + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef std::vector< ::robot_nav_2d_msgs::Pose2DStamped_ , typename std::allocator_traits::template rebind_alloc< ::robot_nav_2d_msgs::Pose2DStamped_ >> _poses_type; + _poses_type poses; + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_ const> ConstPtr; + +}; // struct Path2D_ + +typedef ::robot_nav_2d_msgs::Path2D_ > Path2D; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D > Path2DPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D const> Path2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Path2D_ & lhs, const ::robot_nav_2d_msgs::Path2D_ & rhs) +{ + return lhs.header == rhs.header && + lhs.poses == rhs.poses; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Path2D_ & lhs, const ::robot_nav_2d_msgs::Path2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h new file mode 100644 index 0000000..b607ff5 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h @@ -0,0 +1,72 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Point2D.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct Point2D_ +{ + typedef Point2D_ Type; + + Point2D_() + : x(0.0) + , y(0.0) { + } + Point2D_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D_ const> ConstPtr; + +}; // struct Point2D_ + +typedef ::robot_nav_2d_msgs::Point2D_ > Point2D; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D > Point2DPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D const> Point2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Point2D_ & lhs, const ::robot_nav_2d_msgs::Point2D_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Point2D_ & lhs, const ::robot_nav_2d_msgs::Point2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h new file mode 100644 index 0000000..bd3e5e9 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h @@ -0,0 +1,67 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Polygon2D.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H + + +#include +#include +#include + +#include + +namespace robot_nav_2d_msgs +{ +template +struct Polygon2D_ +{ + typedef Polygon2D_ Type; + + Polygon2D_() + : points() { + } + Polygon2D_(const ContainerAllocator& _alloc) + : points(_alloc) { + (void)_alloc; + } + + + + typedef std::vector< ::robot_nav_2d_msgs::Point2D_ , typename std::allocator_traits::template rebind_alloc< ::robot_nav_2d_msgs::Point2D_ >> _points_type; + _points_type points; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_ const> ConstPtr; + +}; // struct Polygon2D_ + +typedef ::robot_nav_2d_msgs::Polygon2D_ > Polygon2D; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D > Polygon2DPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D const> Polygon2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Polygon2D_ & lhs, const ::robot_nav_2d_msgs::Polygon2D_ & rhs) +{ + return lhs.points == rhs.points; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Polygon2D_ & lhs, const ::robot_nav_2d_msgs::Polygon2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h new file mode 100644 index 0000000..3c631eb --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h @@ -0,0 +1,81 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Polygon2DCollection.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H + + +#include +#include +#include + +#include +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct Polygon2DCollection_ +{ + typedef Polygon2DCollection_ Type; + + Polygon2DCollection_() + : header() + , polygons() + , colors() { + } + Polygon2DCollection_(const ContainerAllocator& _alloc) + : header() + , polygons(_alloc) + , colors(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef std::vector< ::robot_nav_2d_msgs::ComplexPolygon2D_ , typename std::allocator_traits::template rebind_alloc< ::robot_nav_2d_msgs::ComplexPolygon2D_ >> _polygons_type; + _polygons_type polygons; + + typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type; + _colors_type colors; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_ const> ConstPtr; + +}; // struct Polygon2DCollection_ + +typedef ::robot_nav_2d_msgs::Polygon2DCollection_ > Polygon2DCollection; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection > Polygon2DCollectionPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection const> Polygon2DCollectionConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Polygon2DCollection_ & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_ & rhs) +{ + return lhs.header == rhs.header && + lhs.polygons == rhs.polygons && + lhs.colors == rhs.colors; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Polygon2DCollection_ & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h new file mode 100644 index 0000000..998759a --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Polygon2DStamped.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct Polygon2DStamped_ +{ + typedef Polygon2DStamped_ Type; + + Polygon2DStamped_() + : header() + , polygon() { + } + Polygon2DStamped_(const ContainerAllocator& _alloc) + : header() + , polygon(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::robot_nav_2d_msgs::Polygon2D_ _polygon_type; + _polygon_type polygon; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_ const> ConstPtr; + +}; // struct Polygon2DStamped_ + +typedef ::robot_nav_2d_msgs::Polygon2DStamped_ > Polygon2DStamped; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped > Polygon2DStampedPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped const> Polygon2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Polygon2DStamped_ & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.polygon == rhs.polygon; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Polygon2DStamped_ & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h new file mode 100644 index 0000000..c8c013e --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Pose2D32.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct Pose2D32_ +{ + typedef Pose2D32_ Type; + + Pose2D32_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Pose2D32_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32_ const> ConstPtr; + +}; // struct Pose2D32_ + +typedef ::robot_nav_2d_msgs::Pose2D32_ > Pose2D32; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32 > Pose2D32Ptr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32 const> Pose2D32ConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Pose2D32_ & lhs, const ::robot_nav_2d_msgs::Pose2D32_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Pose2D32_ & lhs, const ::robot_nav_2d_msgs::Pose2D32_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h new file mode 100644 index 0000000..c2c2471 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Pose2DStamped.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct Pose2DStamped_ +{ + typedef Pose2DStamped_ Type; + + Pose2DStamped_() + : header() + , pose() { + } + Pose2DStamped_(const ContainerAllocator& _alloc) + : header() + , pose() { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::geometry_msgs::Pose2D _pose_type; + _pose_type pose; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped_ const> ConstPtr; + +}; // struct Pose2DStamped_ + +typedef ::robot_nav_2d_msgs::Pose2DStamped_ > Pose2DStamped; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped > Pose2DStampedPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped const> Pose2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Pose2DStamped_ & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.pose == rhs.pose; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Pose2DStamped_ & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h new file mode 100644 index 0000000..dd2e2e3 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h @@ -0,0 +1,30 @@ +// Generated by gencpp from file robot_nav_2d_msgs/SwitchPlugin.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H + + +#include +#include + + +namespace robot_nav_2d_msgs +{ + +struct SwitchPlugin +{ + +typedef SwitchPluginRequest Request; +typedef SwitchPluginResponse Response; +Request request; +Response response; + +typedef Request RequestType; +typedef Response ResponseType; + +}; // struct SwitchPlugin +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGIN_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h new file mode 100644 index 0000000..5c611bc --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h @@ -0,0 +1,66 @@ +// Generated by gencpp from file robot_nav_2d_msgs/SwitchPluginRequest.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct SwitchPluginRequest_ +{ + typedef SwitchPluginRequest_ Type; + + SwitchPluginRequest_() + : new_plugin() { + } + SwitchPluginRequest_(const ContainerAllocator& _alloc) + : new_plugin(_alloc) { + (void)_alloc; + } + + + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _new_plugin_type; + _new_plugin_type new_plugin; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_ const> ConstPtr; + +}; // struct SwitchPluginRequest_ + +typedef ::robot_nav_2d_msgs::SwitchPluginRequest_ > SwitchPluginRequest; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest > SwitchPluginRequestPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest const> SwitchPluginRequestConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_ & rhs) +{ + return lhs.new_plugin == rhs.new_plugin; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginRequest_ & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h new file mode 100644 index 0000000..4d275e3 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h @@ -0,0 +1,72 @@ +// Generated by gencpp from file robot_nav_2d_msgs/SwitchPluginResponse.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct SwitchPluginResponse_ +{ + typedef SwitchPluginResponse_ Type; + + SwitchPluginResponse_() + : success(false) + , message() { + } + SwitchPluginResponse_(const ContainerAllocator& _alloc) + : success(false) + , message(_alloc) { + (void)_alloc; + } + + + + typedef uint8_t _success_type; + _success_type success; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _message_type; + _message_type message; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_ const> ConstPtr; + +}; // struct SwitchPluginResponse_ + +typedef ::robot_nav_2d_msgs::SwitchPluginResponse_ > SwitchPluginResponse; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse > SwitchPluginResponsePtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse const> SwitchPluginResponseConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_ & rhs) +{ + return lhs.success == rhs.success && + lhs.message == rhs.message; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginResponse_ & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h new file mode 100644 index 0000000..dee7817 --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Twist2D.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct Twist2D_ +{ + typedef Twist2D_ Type; + + Twist2D_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Twist2D_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D_ const> ConstPtr; + +}; // struct Twist2D_ + +typedef ::robot_nav_2d_msgs::Twist2D_ > Twist2D; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D > Twist2DPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D const> Twist2DConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Twist2D_ & lhs, const ::robot_nav_2d_msgs::Twist2D_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Twist2D_ & lhs, const ::robot_nav_2d_msgs::Twist2D_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h new file mode 100644 index 0000000..0ea3b6b --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h @@ -0,0 +1,78 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Twist2D32.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H + + +#include +#include +#include + + +namespace robot_nav_2d_msgs +{ +template +struct Twist2D32_ +{ + typedef Twist2D32_ Type; + + Twist2D32_() + : x(0.0) + , y(0.0) + , theta(0.0) { + } + Twist2D32_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , theta(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _theta_type; + _theta_type theta; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32_ const> ConstPtr; + +}; // struct Twist2D32_ + +typedef ::robot_nav_2d_msgs::Twist2D32_ > Twist2D32; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32 > Twist2D32Ptr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32 const> Twist2D32ConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Twist2D32_ & lhs, const ::robot_nav_2d_msgs::Twist2D32_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.theta == rhs.theta; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Twist2D32_ & lhs, const ::robot_nav_2d_msgs::Twist2D32_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H diff --git a/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h new file mode 100644 index 0000000..6e4d48e --- /dev/null +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h @@ -0,0 +1,74 @@ +// Generated by gencpp from file robot_nav_2d_msgs/Twist2DStamped.msg +// DO NOT EDIT! + + +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H + + +#include +#include +#include + +#include +#include + +namespace robot_nav_2d_msgs +{ +template +struct Twist2DStamped_ +{ + typedef Twist2DStamped_ Type; + + Twist2DStamped_() + : header() + , velocity() { + } + Twist2DStamped_(const ContainerAllocator& _alloc) + : header() + , velocity(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header _header_type; + _header_type header; + + typedef ::robot_nav_2d_msgs::Twist2D_ _velocity_type; + _velocity_type velocity; + + + + + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_ const> ConstPtr; + +}; // struct Twist2DStamped_ + +typedef ::robot_nav_2d_msgs::Twist2DStamped_ > Twist2DStamped; + +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped > Twist2DStampedPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped const> Twist2DStampedConstPtr; + +// constants requiring out of line definition + + + +template +bool operator==(const ::robot_nav_2d_msgs::Twist2DStamped_ & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.velocity == rhs.velocity; +} + +template +bool operator!=(const ::robot_nav_2d_msgs::Twist2DStamped_ & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace robot_nav_2d_msgs + +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H diff --git a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h similarity index 50% rename from src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h rename to src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h index 0a006ae..d2a074f 100644 --- a/src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h +++ b/src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h @@ -1,9 +1,9 @@ -// Generated by gencpp from file nav_2d_msgs/UIntBounds.msg +// Generated by gencpp from file robot_nav_2d_msgs/UIntBounds.msg // DO NOT EDIT! -#ifndef NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H -#define NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H +#ifndef ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H +#define ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H #include @@ -11,7 +11,7 @@ #include -namespace nav_2d_msgs +namespace robot_nav_2d_msgs { template struct UIntBounds_ @@ -49,22 +49,22 @@ struct UIntBounds_ - typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_ > Ptr; - typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_ const> ConstPtr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_ > Ptr; + typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_ const> ConstPtr; }; // struct UIntBounds_ -typedef ::nav_2d_msgs::UIntBounds_ > UIntBounds; +typedef ::robot_nav_2d_msgs::UIntBounds_ > UIntBounds; -typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds > UIntBoundsPtr; -typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds const> UIntBoundsConstPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds > UIntBoundsPtr; +typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds const> UIntBoundsConstPtr; // constants requiring out of line definition template -bool operator==(const ::nav_2d_msgs::UIntBounds_ & lhs, const ::nav_2d_msgs::UIntBounds_ & rhs) +bool operator==(const ::robot_nav_2d_msgs::UIntBounds_ & lhs, const ::robot_nav_2d_msgs::UIntBounds_ & rhs) { return lhs.min_x == rhs.min_x && lhs.min_y == rhs.min_y && @@ -73,12 +73,12 @@ bool operator==(const ::nav_2d_msgs::UIntBounds_ & lhs, con } template -bool operator!=(const ::nav_2d_msgs::UIntBounds_ & lhs, const ::nav_2d_msgs::UIntBounds_ & rhs) +bool operator!=(const ::robot_nav_2d_msgs::UIntBounds_ & lhs, const ::robot_nav_2d_msgs::UIntBounds_ & rhs) { return !(lhs == rhs); } -} // namespace nav_2d_msgs +} // namespace robot_nav_2d_msgs -#endif // NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H +#endif // ROBOT_NAV_2D_MSGS_MESSAGE_UINTBOUNDS_H diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt similarity index 79% rename from src/Libraries/nav_2d_utils/CMakeLists.txt rename to src/Libraries/robot_nav_2d_utils/CMakeLists.txt index a89a66d..836b63c 100755 --- a/src/Libraries/nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10) -project(nav_2d_utils VERSION 1.0.0 LANGUAGES CXX) +project(robot_nav_2d_utils VERSION 1.0.0 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -27,7 +27,7 @@ target_include_directories(conversions ) target_link_libraries(conversions PUBLIC - nav_2d_msgs + robot_nav_2d_msgs geometry_msgs nav_msgs nav_grid @@ -51,7 +51,7 @@ target_include_directories(path_ops ) target_link_libraries(path_ops PUBLIC - nav_2d_msgs + robot_nav_2d_msgs geometry_msgs robot_cpp ) @@ -66,17 +66,17 @@ target_include_directories(polygons if(xmlrpcpp_FOUND) target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS}) target_link_libraries(polygons - PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp PRIVATE ${xmlrpcpp_LIBRARIES}) elseif(XmlRpcCpp_FOUND) target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS}) target_link_libraries(polygons - PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp PRIVATE ${XmlRpcCpp_LIBRARIES}) else() target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS}) target_link_libraries(polygons - PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp PRIVATE ${xmlrpcpp_LIBRARIES}) endif() @@ -109,7 +109,7 @@ target_include_directories(tf_help ) target_link_libraries(tf_help PUBLIC - nav_2d_msgs + robot_nav_2d_msgs geometry_msgs nav_core2 tf3 @@ -120,14 +120,14 @@ set_target_properties(tf_help PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) -# Create an INTERFACE library that represents all nav_2d_utils libraries -add_library(nav_2d_utils INTERFACE) -target_include_directories(nav_2d_utils +# Create an INTERFACE library that represents all robot_nav_2d_utils libraries +add_library(robot_nav_2d_utils INTERFACE) +target_include_directories(robot_nav_2d_utils INTERFACE $ $ ) -target_link_libraries(nav_2d_utils +target_link_libraries(robot_nav_2d_utils INTERFACE conversions path_ops @@ -138,7 +138,7 @@ target_link_libraries(nav_2d_utils ) # Install header files -install(DIRECTORY include/nav_2d_utils +install(DIRECTORY include/robot_nav_2d_utils DESTINATION include FILES_MATCHING PATTERN "*.h") @@ -148,17 +148,17 @@ install(DIRECTORY include/mapbox FILES_MATCHING PATTERN "*.hpp") # Install targets -install(TARGETS nav_2d_utils conversions path_ops polygons bounds tf_help - EXPORT nav_2d_utils-targets +install(TARGETS robot_nav_2d_utils conversions path_ops polygons bounds tf_help + EXPORT robot_nav_2d_utils-targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) # Export targets -install(EXPORT nav_2d_utils-targets - FILE nav_2d_utils-targets.cmake - NAMESPACE nav_2d_utils:: - DESTINATION lib/cmake/nav_2d_utils) +install(EXPORT robot_nav_2d_utils-targets + FILE robot_nav_2d_utils-targets.cmake + NAMESPACE robot_nav_2d_utils:: + DESTINATION lib/cmake/robot_nav_2d_utils) # Print configuration info message(STATUS "=================================") @@ -166,5 +166,5 @@ message(STATUS "Project: ${PROJECT_NAME}") message(STATUS "Version: ${PROJECT_VERSION}") message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help") -message(STATUS "Dependencies: nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost") +message(STATUS "Dependencies: robot_nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost") message(STATUS "=================================") diff --git a/src/Libraries/nav_2d_utils/README.md b/src/Libraries/robot_nav_2d_utils/README.md similarity index 70% rename from src/Libraries/nav_2d_utils/README.md rename to src/Libraries/robot_nav_2d_utils/README.md index c71f8a7..bdf9f21 100755 --- a/src/Libraries/nav_2d_utils/README.md +++ b/src/Libraries/robot_nav_2d_utils/README.md @@ -1,10 +1,10 @@ -# nav_2d_utils +# robot_nav_2d_utils A handful of useful utility functions for nav_core2 packages. * Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds. - * [Conversions](doc/Conversions.md) - Tools for converting between `nav_2d_msgs` and other types. - * OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `nav_2d_msgs::Twist` + * [Conversions](doc/Conversions.md) - Tools for converting between `robot_nav_2d_msgs` and other types. + * OdomSubscriber - subscribes to the standard `nav_msgs::Odometry` message and provides access to the velocity component as a `robot_nav_2d_msgs::Twist` * Parameters - a couple ROS parameter patterns - * PathOps - functions for working with `nav_2d_msgs::Path2D` objects (beyond strict conversion) + * PathOps - functions for working with `robot_nav_2d_msgs::Path2D` objects (beyond strict conversion) * [Plugin Mux](doc/PluginMux.md) - tool for switching between multiple `pluginlib` plugins * [Polygons and Footprints](doc/PolygonsAndFootprints.md) - functions for working with `Polygon2D` objects - * TF Help - Tools for transforming `nav_2d_msgs` and other common operations. + * TF Help - Tools for transforming `robot_nav_2d_msgs` and other common operations. diff --git a/src/Libraries/nav_2d_utils/doc/Conversions.md b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md similarity index 74% rename from src/Libraries/nav_2d_utils/doc/Conversions.md rename to src/Libraries/robot_nav_2d_utils/doc/Conversions.md index 6361563..5588628 100755 --- a/src/Libraries/nav_2d_utils/doc/Conversions.md +++ b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md @@ -1,20 +1,20 @@ -# nav_2d_utils Conversions +# robot_nav_2d_utils Conversions (note: exact syntax differs from below for conciseness, leaving out `const` and `&`) ## Twist Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | | `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)` ## Point Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | | `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)` | `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)` ## Pose Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | | `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)` ||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`| @@ -22,7 +22,7 @@ | `Pose2DStamped stampedPoseToPose2D(tf::Stamped)` | | ## Path Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | | `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)` | `Path2D posesToPath2D(std::vector)` | `nav_msgs::Path poses2DToPath(std::vector, std::string, robot::Time)` @@ -30,15 +30,15 @@ Also, `nav_msgs::Path posesToPath(std::vector)` ## Polygon Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | | `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)` | `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)` ## Info Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | -|`nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(nav_2d_msgs::NavGridInfo)`| +|`robot_nav_2d_msgs::NavGridInfo toMsg(nav_grid::NavGridInfo)`|`nav_grid::NavGridInfo fromMsg(robot_nav_2d_msgs::NavGridInfo)`| | to `nav_grid` info | from `nav_grid` info | | -- | -- | @@ -49,6 +49,6 @@ Also, `nav_msgs::Path posesToPath(std::vector)` | `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`| ## Bounds Transformations -| to `nav_2d_msgs` | from `nav_2d_msgs` | +| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` | | -- | -- | -|`nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(nav_2d_msgs::UIntBounds)`| +|`robot_nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`| diff --git a/src/Libraries/nav_2d_utils/doc/PluginMux.md b/src/Libraries/robot_nav_2d_utils/doc/PluginMux.md similarity index 100% rename from src/Libraries/nav_2d_utils/doc/PluginMux.md rename to src/Libraries/robot_nav_2d_utils/doc/PluginMux.md diff --git a/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md b/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md similarity index 75% rename from src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md rename to src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md index 6d3d5dd..5ecd66b 100755 --- a/src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md +++ b/src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md @@ -1,22 +1,22 @@ -# nav_2d_utils Polygons and Footprints +# robot_nav_2d_utils Polygons and Footprints This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes." ## Polygons and the Parameter Server There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with ``` -nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); +robot_nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); ``` The second two ways involve specifying the points of the polygon individually. This can be done with either a string representing a bracketed array of arrays of doubles, `"[[1.0, 2.2], [3.3, 4.2], ...]"`. This can be read with ``` -nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); +robot_nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); ``` Alternatively, with ROS, you can read the points directly from the parameter server in the form of an `XmlRpcValue`, which should be an array of arrays of doubles, which is read with ``` -nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); +robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); ``` If the `XmlRpcValue` is a string, it will call the `polygonFromString` method. @@ -24,22 +24,22 @@ If the `XmlRpcValue` is a string, it will call the `polygonFromString` method. The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates. ``` -nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); +robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); ``` All of the above methods (except the radius one) can be loaded as appropriate from the parameter server with ``` -nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, +robot_nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name, bool search = true); ``` to include the radius, use the logic in `footprint.h` which either uses "footprint" or "robot_radius" ``` -nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true); +robot_nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true); ``` Polygons can be written to parameters with ``` -void polygontoParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, +void polygontoParams(const robot_nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, bool array_of_arrays = true); ``` diff --git a/src/Libraries/nav_2d_utils/include/mapbox/LICENSE b/src/Libraries/robot_nav_2d_utils/include/mapbox/LICENSE similarity index 100% rename from src/Libraries/nav_2d_utils/include/mapbox/LICENSE rename to src/Libraries/robot_nav_2d_utils/include/mapbox/LICENSE diff --git a/src/Libraries/nav_2d_utils/include/mapbox/NOTES b/src/Libraries/robot_nav_2d_utils/include/mapbox/NOTES similarity index 100% rename from src/Libraries/nav_2d_utils/include/mapbox/NOTES rename to src/Libraries/robot_nav_2d_utils/include/mapbox/NOTES diff --git a/src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp b/src/Libraries/robot_nav_2d_utils/include/mapbox/earcut.hpp similarity index 100% rename from src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp rename to src/Libraries/robot_nav_2d_utils/include/mapbox/earcut.hpp diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h new file mode 100755 index 0000000..043fa77 --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_BOUNDS_H +#define ROBOT_NAV_2D_UTILS_BOUNDS_H + +#include +#include +#include + +using namespace robot; +/** + * @brief A set of utility functions for Bounds objects interacting with other messages/types + */ +namespace robot_nav_2d_utils +{ + + /** + * @brief return a floating point bounds that covers the entire NavGrid + * @param info Info for the NavGrid + * @return bounds covering the entire NavGrid + */ + nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info); + + /** + * @brief return an integral bounds that covers the entire NavGrid + * @param info Info for the NavGrid + * @return bounds covering the entire NavGrid + */ + nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info); + + /** + * @brief Translate real-valued bounds to uint coordinates based on nav_grid info + * @param info Information used when converting the coordinates + * @param bounds floating point bounds object + * @returns corresponding UIntBounds for parameter + */ + nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds); + + /** + * @brief Translate uint bounds to real-valued coordinates based on nav_grid info + * @param info Information used when converting the coordinates + * @param bounds UIntBounds object + * @returns corresponding floating point bounds for parameter + */ + nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds); + + /** + * @brief divide the given bounds up into sub-bounds of roughly equal size + * @param original_bounds The original bounds to divide + * @param n_cols Positive number of columns to divide the bounds into + * @param n_rows Positive number of rows to divide the bounds into + * @return vector of a maximum of n_cols*n_rows nonempty bounds + * @throws std::invalid_argument when n_cols or n_rows is zero + */ + std::vector divideBounds(const nav_core2::UIntBounds &original_bounds, + unsigned int n_cols, unsigned int n_rows); + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_BOUNDS_H diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h new file mode 100755 index 0000000..a4dbda6 --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_CONVERSIONS_H +#define ROBOT_NAV_2D_UTILS_CONVERSIONS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// #include +#include +#include + +namespace robot_nav_2d_utils +{ + // Twist Transformations + ::geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d); + ::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::geometry_msgs::Twist &cmd_vel); + + // Point Transformations + ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point &point); + ::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point); + ::geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point); + ::geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point); + + // Pose Transformations + // ::robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose); + ::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::geometry_msgs::PoseStamped &pose); + ::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d); + ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d); + ::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::geometry_msgs::Pose2D &pose2d, + const ::std::string &frame, const robot::Time &stamp); + + // Path Transformations + ::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path); + nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses); + ::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::geometry_msgs::PoseStamped> &poses); + nav_msgs::Path poses2DToPath(const ::std::vector<::geometry_msgs::Pose2D> &poses, + const ::std::string &frame, const robot::Time &stamp); + nav_msgs::Path pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d); + + // Polygon Transformations + ::geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d); + ::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::geometry_msgs::Polygon &polygon_3d); + ::geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d); + ::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::geometry_msgs::PolygonStamped &polygon_3d); + + // Info Transformations + ::robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info); + nav_grid::NavGridInfo fromMsg(const ::robot_nav_2d_msgs::NavGridInfo &msg); + ::geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info); + ::geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info); + nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const ::std::string &frame); + nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info); + + // Bounds Transformations + ::robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds); + robot::nav_core2::UIntBounds fromMsg(const ::robot_nav_2d_msgs::UIntBounds &msg); + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_CONVERSIONS_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h similarity index 74% rename from src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h index b0f8c0a..3949bcb 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h @@ -32,23 +32,23 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_2D_UTILS_FOOTPRINT_H -#define NAV_2D_UTILS_FOOTPRINT_H +#ifndef ROBOT_NAV_2D_UTILS_FOOTPRINT_H +#define ROBOT_NAV_2D_UTILS_FOOTPRINT_H #include -#include +#include -namespace nav_2d_utils +namespace robot_nav_2d_utils { -/** - * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius - * - * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter - * is present. - */ -nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle& nh, bool write = true); + /** + * @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius + * + * Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter + * is present. + */ + robot_nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle &nh, bool write = true); -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils -#endif // NAV_2D_UTILS_FOOTPRINT_H +#endif // ROBOT_NAV_2D_UTILS_FOOTPRINT_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h similarity index 63% rename from src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h index 3afae5a..ca9a238 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/geometry_help.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h @@ -32,54 +32,54 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_2D_UTILS_GEOMETRY_HELP_H -#define NAV_2D_UTILS_GEOMETRY_HELP_H +#ifndef ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H +#define ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { -/** - * @brief Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1) - * @param pX - * @param pY - * @param x0 - * @param y0 - * @param x1 - * @param y1 - * @return shortest distance from point to line - */ -inline double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) -{ - double A = pX - x0; - double B = pY - y0; - double C = x1 - x0; - double D = y1 - y0; - - double dot = A * C + B * D; - double len_sq = C * C + D * D; - double param = dot / len_sq; - - double xx, yy; - - if (param < 0) + /** + * @brief Distance from point (pX, pY) to closest point on line from (x0, y0) to (x1, y1) + * @param pX + * @param pY + * @param x0 + * @param y0 + * @param x1 + * @param y1 + * @return shortest distance from point to line + */ + inline double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) { - xx = x0; - yy = y0; - } - else if (param > 1) - { - xx = x1; - yy = y1; - } - else - { - xx = x0 + param * C; - yy = y0 + param * D; - } + double A = pX - x0; + double B = pY - y0; + double C = x1 - x0; + double D = y1 - y0; - return hypot(pX - xx, pY - yy); -} -} // namespace nav_2d_utils + double dot = A * C + B * D; + double len_sq = C * C + D * D; + double param = dot / len_sq; -#endif // NAV_2D_UTILS_GEOMETRY_HELP_H + double xx, yy; + + if (param < 0) + { + xx = x0; + yy = y0; + } + else if (param > 1) + { + xx = x1; + yy = y1; + } + else + { + xx = x0 + param * C; + yy = y0 + param * D; + } + + return hypot(pX - xx, pY - yy); + } +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_GEOMETRY_HELP_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h similarity index 54% rename from src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h index 91f1af6..f472d58 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h @@ -32,56 +32,56 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_2D_UTILS_ODOM_SUBSCRIBER_H -#define NAV_2D_UTILS_ODOM_SUBSCRIBER_H +#ifndef ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H +#define ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H -#include +#include #include -#include +#include #include #include #include #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { -/** - * @class OdomSubscriber - * Wrapper for some common odometry operations. Subscribes to the topic with a mutex. - */ -class OdomSubscriber -{ -public: /** - * @brief Constructor that subscribes to an Odometry topic - * - * @param nh NodeHandle for creating subscriber - * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. + * @class OdomSubscriber + * Wrapper for some common odometry operations. Subscribes to the topic with a mutex. */ - explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom") + class OdomSubscriber { - std::string odom_topic; - // nh.param("odom_topic", odom_topic, default_topic); - // odom_sub_ = nh.subscribe(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1)); - } + public: + /** + * @brief Constructor that subscribes to an Odometry topic + * + * @param nh NodeHandle for creating subscriber + * @param default_topic Name of the topic that will be loaded of the odom_topic param is not set. + */ + explicit OdomSubscriber(robot::NodeHandle &nh, std::string default_topic = "odom") + { + std::string odom_topic; + // nh.param("odom_topic", odom_topic, default_topic); + // odom_sub_ = nh.subscribe(odom_topic, 1, boost::bind(&OdomSubscriber::odomCallback, this, _1)); + } - inline nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; } - inline nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; } + inline robot_nav_2d_msgs::Twist2D getTwist() { return odom_vel_.velocity; } + inline robot_nav_2d_msgs::Twist2DStamped getTwistStamped() { return odom_vel_; } -protected: - void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) - { - std::cout << "odom received!" << std::endl; - boost::mutex::scoped_lock lock(odom_mutex_); - odom_vel_.header = msg->header; - odom_vel_.velocity = twist3Dto2D(msg->twist.twist); - } + protected: + void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) + { + std::cout << "odom received!" << std::endl; + boost::mutex::scoped_lock lock(odom_mutex_); + odom_vel_.header = msg->header; + odom_vel_.velocity = twist3Dto2D(msg->twist.twist); + } - nav_2d_msgs::Twist2DStamped odom_vel_; - boost::mutex odom_mutex_; -}; + robot_nav_2d_msgs::Twist2DStamped odom_vel_; + boost::mutex odom_mutex_; + }; -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils -#endif // NAV_2D_UTILS_ODOM_SUBSCRIBER_H +#endif // ROBOT_NAV_2D_UTILS_ODOM_SUBSCRIBER_H diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h new file mode 100755 index 0000000..70bae3a --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_PARAMETERS_H +#define ROBOT_NAV_2D_UTILS_PARAMETERS_H + +#include +#include +#include +#include + +namespace robot_nav_2d_utils +{ + + /** + * @brief Search for a parameter and load it, or use the default value + * + * This templated function shortens a commonly used ROS pattern in which you + * search for a parameter and get its value if it exists, otherwise returning a default value. + * + * @param nh NodeHandle to start the parameter search from + * @param param_name Name of the parameter to search for + * @param default_value Value to return if not found + * @return Value of parameter if found, otherwise the default_value + */ + template + param_t searchAndGetParam(const robot::NodeHandle &nh, const std::string ¶m_name, const param_t &default_value) + { + std::string resolved_name; + if (nh.searchParam(param_name, resolved_name)) + { + param_t value = default_value; + robot::NodeHandle nh_private = robot::NodeHandle("~"); + nh_private.param(resolved_name, value, default_value); + return value; + } + return default_value; + } + + /** + * @brief Load a parameter from one of two namespaces. Complain if it uses the old name. + * @param nh NodeHandle to look for the parameter in + * @param current_name Parameter name that is current, i.e. not deprecated + * @param old_name Deprecated parameter name + * @param default_value If neither parameter is present, return this value + * @return The value of the parameter or the default value + */ + template + param_t loadParameterWithDeprecation(const robot::NodeHandle &nh, const std::string current_name, + const std::string old_name, const param_t &default_value) + { + param_t value; + if (nh.hasParam(current_name)) + { + nh.getParam(current_name, value, default_value); + return value; + } + if (nh.hasParam(old_name)) + { + robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); + nh.getParam(old_name, value, default_value); + return value; + } + return default_value; + } + + /** + * @brief If a deprecated parameter exists, complain and move to its new location + * @param nh NodeHandle to look for the parameter in + * @param current_name Parameter name that is current, i.e. not deprecated + * @param old_name Deprecated parameter name + */ + template + void moveDeprecatedParameter(const robot::NodeHandle &nh, const std::string current_name, const std::string old_name) + { + if (!nh.hasParam(old_name)) + return; + + param_t value; + robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str()); + value = nh.param(old_name); + nh.setParam(current_name, value); + } + + /** + * @brief Move a parameter from one place to another + * + * For use loading old parameter structures into new places. + * + * If the new name already has a value, don't move the old value there. + * + * @param nh NodeHandle for loading/saving params + * @param old_name Parameter name to move/remove + * @param current_name Destination parameter name + * @param default_value Value to save in the new name if old parameter is not found. + * @param should_delete If true, whether to delete the parameter from the old name + */ + template + void moveParameter(const robot::NodeHandle &nh, std::string old_name, + std::string current_name, param_t default_value, bool should_delete = true) + { + // if (nh.hasParam(current_name)) + // { + // if (should_delete) + // nh.deleteParam(old_name); + // return; + // } + // XmlRpc::XmlRpcValue value; + // if (nh.hasParam(old_name)) + // { + // nh.getParam(old_name, value); + // if (should_delete) nh.deleteParam(old_name); + // } + // else + // value = default_value; + + // nh.setParam(current_name, value); + } + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_PARAMETERS_H diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h new file mode 100755 index 0000000..a385587 --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h @@ -0,0 +1,88 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_PATH_OPS_H +#define ROBOT_NAV_2D_UTILS_PATH_OPS_H + +#include + +namespace robot_nav_2d_utils +{ + /** + * @brief Calculate the linear distance between two poses + */ + double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1); + + /** + * @brief Calculate the length of the plan, starting from the given index + */ + double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const unsigned int start_index = 0); + + /** + * @brief Calculate the length of the plan from the pose on the plan closest to the given pose + */ + double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose); + + /** + * @brief Increase plan resolution to match that of the costmap by adding points linearly between points + * + * @param global_plan_in input plan + * @param resolution desired distance between waypoints + * @return Higher resolution plan + */ + robot_nav_2d_msgs::Path2D adjustPlanResolution(const robot_nav_2d_msgs::Path2D &global_plan_in, double resolution); + + /** + * @brief Decrease the length of the plan by eliminating colinear points + * + * Uses the Ramer Douglas Peucker algorithm. Ignores theta values + * + * @param input_path Path to compress + * @param epsilon maximum allowable error. Increase for greater compression. + * @return Path2D with possibly fewer poses + */ + robot_nav_2d_msgs::Path2D compressPlan(const robot_nav_2d_msgs::Path2D &input_path, double epsilon = 0.1); + + /** + * @brief Convenience function to add a pose to a path in one line. + * @param path Path to add to + * @param x x-coordinate + * @param y y-coordinate + * @param theta theta (if needed) + */ + void addPose(robot_nav_2d_msgs::Path2D &path, double x, double y, double theta = 0.0); + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_PATH_OPS_H diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h new file mode 100755 index 0000000..b1a4938 --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h @@ -0,0 +1,275 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H +#define ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H + +#include +#include +#include +#include +#include +#include +#include + +namespace robot_nav_2d_utils +{ + /** + * @class PluginMux + * @brief An organizer for switching between multiple different plugins of the same type + * + * The different plugins are specified using a list of strings on the parameter server, each of which is a namespace. + * The specific type and additional parameters for each plugin are specified on the parameter server in that namespace. + * All the plugins are loaded initially, but only one is the "current" plugin at any particular time, which is published + * on a latched topic AND stored on the ROS parameter server. You can switch which plugin is current either through a + * C++ or ROS interface. + */ + template + class PluginMux + { + public: + /** + * @brief Main Constructor - Loads plugin(s) and sets up ROS interfaces + * + * @param plugin_package The package of the plugin type + * @param plugin_class The class name for the plugin type + * @param parameter_name Name of parameter for the namespaces. + * @param default_value If class name is not specified, which plugin should be loaded + * @param ros_name ROS name for setting up topic and parameter + * @param switch_service_name ROS name for setting up the ROS service + */ + PluginMux(const std::string &plugin_package, const std::string &plugin_class, + const std::string ¶meter_name, const std::string &default_value, + const std::string &ros_name = "current_plugin", const std::string &switch_service_name = "switch_plugin"); + + /** + * @brief Create an instance of the given plugin_class_name and save it with the given plugin_name + * @param plugin_name Namespace for the new plugin + * @param plugin_class_name Class type name for the new plugin + */ + void addPlugin(const std::string &plugin_name, const std::string &plugin_class_name); + + /** + * @brief C++ Interface for switching to a given plugin + * + * @param name Namespace to set current plugin to + * @return true if that namespace exists and is loaded properly + */ + bool usePlugin(const std::string &name) + { + // If plugin with given name doesn't exist, return false + if (plugins_.count(name) == 0) + return false; + + if (switch_callback_) + { + switch_callback_(current_plugin_, name); + } + + // Switch Mux + current_plugin_ = name; + + // Update ROS info + std_msgs::String str_msg; + str_msg.data = current_plugin_; + current_plugin_pub_.publish(str_msg); + private_nh_.setParam(ros_name_, current_plugin_); + + return true; + } + + /** + * @brief Get the Current Plugin Name + * @return Name of the current plugin + */ + std::string getCurrentPluginName() const + { + return current_plugin_; + } + + /** + * @brief Check to see if a plugin exists + * @param name Namespace to set current plugin to + * @return True if the plugin exists + */ + bool hasPlugin(const std::string &name) const + { + return plugins_.find(name) != plugins_.end(); + } + + /** + * @brief Get the Specified Plugin + * @param name Name of plugin to get + * @return Reference to specified plugin + */ + T &getPlugin(const std::string &name) + { + if (!hasPlugin(name)) + throw std::invalid_argument("Plugin not found"); + return *plugins_[name]; + } + + /** + * @brief Get the Current Plugin + * @return Reference to current plugin + */ + T &getCurrentPlugin() + { + return getPlugin(current_plugin_); + } + + /** + * @brief Get the current list of plugin names + */ + std::vector getPluginNames() const + { + std::vector names; + for (auto &kv : plugins_) + { + names.push_back(kv.first); + } + return names; + } + + /** + * @brief alias for the function-type of the callback fired when the plugin switches. + * + * The first parameter will be the namespace of the plugin that was previously used. + * The second parameter will be the namespace of the plugin that is being switched to. + */ + using SwitchCallback = std::function; + + /** + * @brief Set the callback fired when the plugin switches + * + * In addition to switching which plugin is being used via directly calling `usePlugin` + * a switch can also be triggered externally via ROS service. In such a case, other classes + * may want to know when this happens. This is accomplished using the SwitchCallback, which + * will be called regardless of how the plugin is switched. + */ + void setSwitchCallback(SwitchCallback switch_callback) { switch_callback_ = switch_callback; } + + protected: + /** + * @brief ROS Interface for Switching Plugins + */ + bool switchPluginService(robot_nav_2d_msgs::SwitchPlugin::Request &req, robot_nav_2d_msgs::SwitchPlugin::Response &resp) + { + std::string name = req.new_plugin; + if (usePlugin(name)) + { + resp.success = true; + resp.message = "Loaded plugin namespace " + name + "."; + } + else + { + resp.success = false; + resp.message = "Namespace " + name + " not configured!"; + } + return true; + } + + // Plugin Management + pluginlib::ClassLoader plugin_loader_; + std::map> plugins_; + std::string current_plugin_; + + // ROS Interface + robot::ServiceServer switch_plugin_srv_; + robot::Publisher current_plugin_pub_; + robot::NodeHandle private_nh_; + std::string ros_name_; + + // Switch Callback + SwitchCallback switch_callback_; + }; + + // ********************************************************************************************************************* + // ****************** Implementation of Templated Methods ************************************************************** + // ********************************************************************************************************************* + template + PluginMux::PluginMux(const std::string &plugin_package, const std::string &plugin_class, + const std::string ¶meter_name, const std::string &default_value, + const std::string &ros_name, const std::string &switch_service_name) + : plugin_loader_(plugin_package, plugin_class), private_nh_("~"), ros_name_(ros_name), switch_callback_(nullptr) + { + // Create the latched publisher + current_plugin_pub_ = private_nh_.advertise(ros_name_, 1, true); + + // Load Plugins + std::string plugin_class_name; + std::vector plugin_namespaces; + private_nh_.getParam(parameter_name, plugin_namespaces); + if (plugin_namespaces.size() == 0) + { + // If no namespaces are listed, use the name of the default class as the singular namespace. + std::string plugin_name = plugin_loader_.getName(default_value); + plugin_namespaces.push_back(plugin_name); + } + + for (const std::string &the_namespace : plugin_namespaces) + { + // Load the class name from namespace/plugin_class, or use default value + private_nh_.param(std::string(the_namespace + "/plugin_class"), plugin_class_name, default_value); + addPlugin(the_namespace, plugin_class_name); + } + + // By default, use the first one as current + usePlugin(plugin_namespaces[0]); + + // Now that the plugins are created, advertise the service if there are multiple + if (plugin_namespaces.size() > 1) + { + switch_plugin_srv_ = private_nh_.advertiseService(switch_service_name, &PluginMux::switchPluginService, this); + } + } + + template + void PluginMux::addPlugin(const std::string &plugin_name, const std::string &plugin_class_name) + { + try + { + plugins_[plugin_name] = plugin_loader_.createInstance(plugin_class_name); + } + catch (const pluginlib::PluginlibException &ex) + { + ROS_FATAL_NAMED("PluginMux", + "Failed to load the plugin: %s. Exception: %s", plugin_name.c_str(), ex.what()); + exit(EXIT_FAILURE); + } + } + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_PLUGIN_MUX_H diff --git a/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h similarity index 77% rename from src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h rename to src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h index 80ca500..35b78b2 100755 --- a/src/Libraries/nav_2d_utils/include/nav_2d_utils/polygons.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h @@ -32,17 +32,17 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_2D_UTILS_POLYGONS_H -#define NAV_2D_UTILS_POLYGONS_H +#ifndef ROBOT_NAV_2D_UTILS_POLYGONS_H +#define ROBOT_NAV_2D_UTILS_POLYGONS_H -#include -#include +#include +#include #include #include #include #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { /** @@ -71,7 +71,7 @@ std::vector > parseVVD(const std::string& input); * @param num_points Number of points in the resulting polygon * @return A rotationally symmetric polygon with the specified number of vertices */ -nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); +robot_nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int num_points = 16); /** * @brief Make a polygon from the given string. @@ -80,7 +80,7 @@ nav_2d_msgs::Polygon2D polygonFromRadius(const double radius, const unsigned int * @param polygon_string The string to parse * @return Polygon2D */ -nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); +robot_nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); // /** // * @brief Load a polygon from a parameter, whether it is a string or array, or two arrays @@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); // * @param search Whether to search up the namespace for the parameter name // * @return Loaded polygon // */ -// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name, +// robot_nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name, // bool search = true); /** @@ -99,7 +99,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string); * 3 or more elements, and the sub-arrays should all have exactly 2 elements * (x and y coordinates). */ -nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); +robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); /** * @brief Create XMLRPC Value for writing the polygon to the parameter server @@ -107,7 +107,7 @@ nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlrpc); * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays * @return XmlRpcValue */ -XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true); +XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays = true); // /** // * @brief Save a polygon to a parameter @@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool // * @param parameter_name Name of the parameter // * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays // */ -// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name, +// void polygonToParams(const robot_nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name, // bool array_of_arrays = true); /** @@ -125,7 +125,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool * @param xs Array of doubles representing x coordinates, at least three elements long * @param ys Array of doubles representing y coordinates, matching length of xs */ -nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); +robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys); /** * @brief Create two parallel arrays from a polygon @@ -134,12 +134,12 @@ nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, * @param[out] xs Array of doubles representing x coordinates, to be populated * @param[out] ys Array of doubles representing y coordinates, to be populated */ -void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys); +void polygonToParallelArrays(const robot_nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys); /** * @brief check if two polygons are equal. Used in testing */ -bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D& polygon1); +bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msgs::Polygon2D& polygon1); /** * @brief Translate and rotate a polygon to a new pose @@ -147,7 +147,7 @@ bool equals(const nav_2d_msgs::Polygon2D& polygon0, const nav_2d_msgs::Polygon2D * @param pose The x, y and theta to use when moving the polygon * @return A new moved polygon */ -nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon, +robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon, const geometry_msgs::Pose2D& pose); /** @@ -160,7 +160,7 @@ nav_2d_msgs::Polygon2D movePolygonToPose(const nav_2d_msgs::Polygon2D& polygon, * @param y y coordinate * @return true if point is inside polygon */ -bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const double y); +bool isInside(const robot_nav_2d_msgs::Polygon2D& polygon, const double x, const double y); /** * @brief Calculate the minimum and maximum distance from (0, 0) to any point on the polygon @@ -168,7 +168,7 @@ bool isInside(const nav_2d_msgs::Polygon2D& polygon, const double x, const doubl * @param[out] min_dist * @param[out] max_dist */ -void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist); +void calculateMinAndMaxDistances(const robot_nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist); /** * @brief Decompose a complex polygon into a set of triangles. @@ -180,7 +180,7 @@ void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& * @param polygon The complex polygon to deconstruct * @return A vector of points where each set of three points represents a triangle */ -std::vector triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon); +std::vector triangulate(const robot_nav_2d_msgs::ComplexPolygon2D& polygon); /** * @brief Decompose a simple polygon into a set of triangles. @@ -192,8 +192,8 @@ std::vector triangulate(const nav_2d_msgs::ComplexPolygon2 * @param polygon The polygon to deconstruct * @return A vector of points where each set of three points represents a triangle */ -std::vector triangulate(const nav_2d_msgs::Polygon2D& polygon); +std::vector triangulate(const robot_nav_2d_msgs::Polygon2D& polygon); -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils -#endif // NAV_2D_UTILS_POLYGONS_H +#endif // ROBOT_NAV_2D_UTILS_POLYGONS_H 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 new file mode 100755 index 0000000..8930ab0 --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROBOT_NAV_2D_UTILS_TF_HELP_H +#define ROBOT_NAV_2D_UTILS_TF_HELP_H + +#include +#include +#include +#include + +namespace robot_nav_2d_utils +{ + /** + * @brief Transform a PoseStamped from one frame to another while catching exceptions + * + * Also returns immediately if the frames are equal. + * @param tf Smart pointer to TFListener + * @param frame Frame to transform the pose into + * @param in_pose Pose to transform + * @param out_pose Place to store the resulting transformed pose + * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. + * @return True if successful transform + */ + bool transformPose(const TFListenerPtr tf, const ::std::string frame, + const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose, + const bool extrapolation_fallback = true); + + /** + * @brief Transform a Pose2DStamped from one frame to another while catching exceptions + * + * Also returns immediately if the frames are equal. + * @param tf Smart pointer to TFListener + * @param frame Frame to transform the pose into + * @param in_pose Pose to transform + * @param out_pose Place to store the resulting transformed pose + * @param extrapolation_fallback If true, if there is an ExtrapolationException, allow looking up the latest timestamp instead. + * @return True if successful transform + */ + bool transformPose(const TFListenerPtr tf, const ::std::string frame, + const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose, + const bool extrapolation_fallback = true); + + /** + * @brief Transform a Pose2DStamped into another frame + * + * Note that this returns a transformed pose + * regardless of whether the transform was successfully performed. + * + * @param tf Smart pointer to TFListener + * @param pose Pose to transform + * @param frame_id Frame to transform the pose into + * @return The resulting transformed pose + */ + ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose, + const ::std::string &frame_id); + +} // namespace robot_nav_2d_utils + +#endif // ROBOT_NAV_2D_UTILS_TF_HELP_H diff --git a/src/Libraries/nav_2d_utils/package.xml b/src/Libraries/robot_nav_2d_utils/package.xml similarity index 77% rename from src/Libraries/nav_2d_utils/package.xml rename to src/Libraries/robot_nav_2d_utils/package.xml index 0d63c96..1d8d44b 100755 --- a/src/Libraries/nav_2d_utils/package.xml +++ b/src/Libraries/robot_nav_2d_utils/package.xml @@ -1,9 +1,9 @@ - nav_2d_utils + robot_nav_2d_utils 0.7.10 - nav_2d_utils is the second generation of the transform library, which lets - the user keep track of multiple coordinate frames over time. nav_2d_utils + robot_nav_2d_utils is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_nav_2d_utils maintains the relationship between coordinate frames in a tree structure buffered in time, and lets the user transform points, vectors, etc between any two coordinate frames at any desired @@ -15,7 +15,7 @@ Tully Foote BSD - http://www.ros.org/wiki/nav_2d_utils + http://www.ros.org/wiki/robot_nav_2d_utils catkin diff --git a/src/Libraries/nav_2d_utils/setup.py b/src/Libraries/robot_nav_2d_utils/setup.py similarity index 85% rename from src/Libraries/nav_2d_utils/setup.py rename to src/Libraries/robot_nav_2d_utils/setup.py index 77852f4..c9612e7 100755 --- a/src/Libraries/nav_2d_utils/setup.py +++ b/src/Libraries/robot_nav_2d_utils/setup.py @@ -4,7 +4,7 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup package_info = generate_distutils_setup( - packages=['nav_2d_utils'], + packages=['robot_nav_2d_utils'], package_dir={'': 'src'} ) diff --git a/src/Libraries/robot_nav_2d_utils/src/bounds.cpp b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp new file mode 100755 index 0000000..728edfe --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp @@ -0,0 +1,103 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include + +using namespace robot; +namespace robot_nav_2d_utils +{ + nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info) + { + return nav_core2::Bounds(info.origin_x, info.origin_y, + info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height); + } + + nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info) + { + // bounds are inclusive, so we subtract one + return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1); + } + + nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds) + { + unsigned int g_min_x, g_min_y, g_max_x, g_max_y; + worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y); + worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y); + return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y); + } + + nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds) + { + double min_x, min_y, max_x, max_y; + gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y); + gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y); + return nav_core2::Bounds(min_x, min_y, max_x, max_y); + } + + std::vector divideBounds(const nav_core2::UIntBounds &original_bounds, + unsigned int n_cols, unsigned int n_rows) + { + if (n_cols * n_rows == 0) + { + throw std::invalid_argument("Number of rows and columns must be positive (not zero)"); + } + unsigned int full_width = original_bounds.getWidth(), + full_height = original_bounds.getHeight(); + + unsigned int small_width = static_cast(ceil(static_cast(full_width) / n_cols)), + small_height = static_cast(ceil(static_cast(full_height) / n_rows)); + + std::vector divided; + + for (unsigned int row = 0; row < n_rows; row++) + { + unsigned int min_y = original_bounds.getMinY() + small_height * row; + unsigned int max_y = std::min(min_y + small_height - 1, original_bounds.getMaxY()); + + for (unsigned int col = 0; col < n_cols; col++) + { + unsigned int min_x = original_bounds.getMinX() + small_width * col; + unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX()); + nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y); + if (!sub.isEmpty()) + divided.push_back(sub); + } + } + return divided; + } +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp new file mode 100755 index 0000000..666ebcc --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp @@ -0,0 +1,336 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +namespace robot_nav_2d_utils +{ + + geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d) + { + geometry_msgs::Twist cmd_vel; + cmd_vel.linear.x = cmd_vel_2d.x; + cmd_vel.linear.y = cmd_vel_2d.y; + cmd_vel.angular.z = cmd_vel_2d.theta; + return cmd_vel; + } + + robot_nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist &cmd_vel) + { + robot_nav_2d_msgs::Twist2D cmd_vel_2d; + cmd_vel_2d.x = cmd_vel.linear.x; + cmd_vel_2d.y = cmd_vel.linear.y; + cmd_vel_2d.theta = cmd_vel.angular.z; + return cmd_vel_2d; + } + + robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point &point) + { + robot_nav_2d_msgs::Point2D output; + output.x = point.x; + output.y = point.y; + return output; + } + + robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32 &point) + { + robot_nav_2d_msgs::Point2D output; + output.x = point.x; + output.y = point.y; + return output; + } + + geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point) + { + geometry_msgs::Point output; + output.x = point.x; + output.y = point.y; + return output; + } + + geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point) + { + geometry_msgs::Point32 output; + output.x = point.x; + output.y = point.y; + return output; + } + + // robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped& pose) + // { + // robot_nav_2d_msgs::Pose2DStamped pose2d; + // pose2d.header.stamp = pose.stamp_; + // pose2d.header.frame_id = pose.frame_id_; + // pose2d.pose.x = pose.getOrigin().getX(); + // pose2d.pose.y = pose.getOrigin().getY(); + // pose2d.pose.theta = tf::getYaw(pose.getRotation()); + // return pose2d; + // } + + robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose) + { + robot_nav_2d_msgs::Pose2DStamped pose2d; + 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); + return pose2d; + } + + geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D &pose2d) + { + geometry_msgs::Pose pose; + pose.position.x = pose2d.x; + pose.position.y = pose2d.y; + // pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); + return pose; + } + + geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d) + { + geometry_msgs::PoseStamped pose; + pose.header = pose2d.header; + pose.pose = pose2DToPose(pose2d.pose); + return pose; + } + + // geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d, + // const std::string& frame, const robot::Time& stamp) + // { + // geometry_msgs::PoseStamped pose; + // pose.header.frame_id = frame; + // pose.header.stamp = stamp; + // pose.pose.position.x = pose2d.x; + // pose.pose.position.y = pose2d.y; + // // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta); + // return pose; + // } + + robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path) + { + robot_nav_2d_msgs::Path2D path2d; + path2d.header = path.header; + robot_nav_2d_msgs::Pose2DStamped stamped_2d; + path2d.poses.resize(path.poses.size()); + for (unsigned int i = 0; i < path.poses.size(); i++) + { + stamped_2d = poseStampedToPose2D(path.poses[i]); + path2d.poses[i] = stamped_2d; + } + return path2d; + } + + nav_msgs::Path posesToPath(const std::vector &poses) + { + nav_msgs::Path path; + if (poses.empty()) + return path; + + path.poses.resize(poses.size()); + path.header.frame_id = poses[0].header.frame_id; + path.header.stamp = poses[0].header.stamp; + for (unsigned int i = 0; i < poses.size(); i++) + { + path.poses[i] = poses[i]; + } + return path; + } + + robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector &poses) + { + robot_nav_2d_msgs::Path2D path; + if (poses.empty()) + return path; + + robot_nav_2d_msgs::Pose2DStamped pose; + path.poses.resize(poses.size()); + path.header.frame_id = poses[0].header.frame_id; + path.header.stamp = poses[0].header.stamp; + for (unsigned int i = 0; i < poses.size(); i++) + { + pose = poseStampedToPose2D(poses[i]); + path.poses[i] = pose; + } + return path; + } + + // nav_msgs::Path poses2DToPath(const std::vector& poses, + // const std::string& frame, const robot::Time& stamp) + // { + // nav_msgs::Path path; + // path.poses.resize(poses.size()); + // path.header.frame_id = frame; + // path.header.stamp = stamp; + // for (unsigned int i = 0; i < poses.size(); i++) + // { + // path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp); + // } + // return path; + // } + + nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d) + { + nav_msgs::Path path; + path.header = path2d.header; + path.poses.resize(path2d.poses.size()); + for (unsigned int i = 0; i < path.poses.size(); i++) + { + path.poses[i].header = path2d.header; + path.poses[i].pose = pose2DToPose(path2d.poses[i].pose); + } + return path; + } + + geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d) + { + geometry_msgs::Polygon polygon; + polygon.points.reserve(polygon_2d.points.size()); + for (const auto &pt : polygon_2d.points) + { + polygon.points.push_back(pointToPoint32(pt)); + } + return polygon; + } + + robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon &polygon_3d) + { + robot_nav_2d_msgs::Polygon2D polygon; + polygon.points.reserve(polygon_3d.points.size()); + for (const auto &pt : polygon_3d.points) + { + polygon.points.push_back(pointToPoint2D(pt)); + } + return polygon; + } + + geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d) + { + geometry_msgs::PolygonStamped polygon; + polygon.header = polygon_2d.header; + polygon.polygon = polygon2Dto3D(polygon_2d.polygon); + return polygon; + } + + robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped &polygon_3d) + { + robot_nav_2d_msgs::Polygon2DStamped polygon; + polygon.header = polygon_3d.header; + polygon.polygon = polygon3Dto2D(polygon_3d.polygon); + return polygon; + } + + robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info) + { + robot_nav_2d_msgs::NavGridInfo msg; + msg.width = info.width; + msg.height = info.height; + msg.resolution = info.resolution; + msg.frame_id = info.frame_id; + msg.origin_x = info.origin_x; + msg.origin_y = info.origin_y; + return msg; + } + + nav_grid::NavGridInfo fromMsg(const robot_nav_2d_msgs::NavGridInfo &msg) + { + nav_grid::NavGridInfo info; + info.width = msg.width; + info.height = msg.height; + info.resolution = msg.resolution; + info.frame_id = msg.frame_id; + info.origin_x = msg.origin_x; + info.origin_y = msg.origin_y; + return info; + } + + nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata, const std::string &frame) + { + nav_grid::NavGridInfo info; + info.frame_id = frame; + info.resolution = metadata.resolution; + info.width = metadata.width; + 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; + // } + return info; + } + + geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info) + { + geometry_msgs::Pose origin; + origin.position.x = info.origin_x; + origin.position.y = info.origin_y; + origin.orientation.w = 1.0; + return origin; + } + + geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info) + { + geometry_msgs::Pose2D origin; + origin.x = info.origin_x; + origin.y = info.origin_y; + return origin; + } + + nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info) + { + nav_msgs::MapMetaData metadata; + metadata.resolution = info.resolution; + metadata.width = info.width; + metadata.height = info.height; + metadata.origin = getOrigin3D(info); + return metadata; + } + + robot_nav_2d_msgs::UIntBounds toMsg(const robot::nav_core2::UIntBounds &bounds) + { + robot_nav_2d_msgs::UIntBounds msg; + msg.min_x = bounds.getMinX(); + msg.min_y = bounds.getMinY(); + msg.max_x = bounds.getMaxX(); + msg.max_y = bounds.getMaxY(); + return msg; + } + + robot::nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg) + { + return robot::nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y); + } + +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/footprint.cpp b/src/Libraries/robot_nav_2d_utils/src/footprint.cpp similarity index 64% rename from src/Libraries/nav_2d_utils/src/footprint.cpp rename to src/Libraries/robot_nav_2d_utils/src/footprint.cpp index 92ac1ea..f0ed1b8 100755 --- a/src/Libraries/nav_2d_utils/src/footprint.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/footprint.cpp @@ -31,37 +31,37 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - -#include -#include +#include +#include +#include #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { -nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write) -{ - std::string full_param_name; - nav_2d_msgs::Polygon2D footprint; + robot_nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node &nh, bool write) + { + std::string full_param_name; + robot_nav_2d_msgs::Polygon2D footprint; - // if (nh.searchParam("footprint", full_param_name)) - // { - // // footprint = polygonFromParams(nh, full_param_name, false); - // if (write) - // { - // nh.setParam("footprint", polygonToXMLRPC(footprint)); - // } - // } - // else if (nh.searchParam("robot_radius", full_param_name)) - // { - // double robot_radius = 0.0; - // nh.getParam(full_param_name, robot_radius); - // footprint = polygonFromRadius(robot_radius); - // if (write) - // { - // nh.setParam("robot_radius", robot_radius); - // } - // } - return footprint; -} + // if (nh.searchParam("footprint", full_param_name)) + // { + // // footprint = polygonFromParams(nh, full_param_name, false); + // if (write) + // { + // nh.setParam("footprint", polygonToXMLRPC(footprint)); + // } + // } + // else if (nh.searchParam("robot_radius", full_param_name)) + // { + // double robot_radius = 0.0; + // nh.getParam(full_param_name, robot_radius); + // footprint = polygonFromRadius(robot_radius); + // if (write) + // { + // nh.setParam("robot_radius", robot_radius); + // } + // } + return footprint; + } -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/robot_nav_2d_utils/src/path_ops.cpp b/src/Libraries/robot_nav_2d_utils/src/path_ops.cpp new file mode 100755 index 0000000..accd82c --- /dev/null +++ b/src/Libraries/robot_nav_2d_utils/src/path_ops.cpp @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +namespace robot_nav_2d_utils +{ + + double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1) + { + return hypot(pose0.x - pose1.x, pose0.y - pose1.y); + } + + double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const unsigned int start_index) + { + double length = 0.0; + for (unsigned int i = start_index + 1; i < plan.poses.size(); i++) + { + length += poseDistance(plan.poses[i - 1].pose, plan.poses[i].pose); + } + return length; + } + + double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose) + { + if (plan.poses.size() == 0) + return 0.0; + + unsigned int closest_index = 0; + double closest_distance = poseDistance(plan.poses[0].pose, query_pose); + for (unsigned int i = 1; i < plan.poses.size(); i++) + { + double distance = poseDistance(plan.poses[i].pose, query_pose); + if (closest_distance > distance) + { + closest_index = i; + closest_distance = distance; + } + } + return getPlanLength(plan, closest_index); + } + + robot_nav_2d_msgs::Path2D adjustPlanResolution(const robot_nav_2d_msgs::Path2D &global_plan_in, double resolution) + { + robot_nav_2d_msgs::Path2D global_plan_out; + global_plan_out.header = global_plan_in.header; + if (global_plan_in.poses.size() == 0) + { + return global_plan_out; + } + + robot_nav_2d_msgs::Pose2DStamped last_stp = global_plan_in.poses[0]; + global_plan_out.poses.push_back(last_stp); + + double sq_resolution = resolution * resolution; + geometry_msgs::Pose2D last = last_stp.pose; + for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i) + { + geometry_msgs::Pose2D loop = global_plan_in.poses[i].pose; + double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y); + if (sq_dist > sq_resolution) + { + // add points in-between + double diff = sqrt(sq_dist) - resolution; + double steps_double = ceil(diff / resolution) + 1.0; + int steps = static_cast(steps_double); + + double delta_x = (loop.x - last.x) / steps_double; + double delta_y = (loop.y - last.y) / steps_double; + double delta_t = (loop.theta - last.theta) / steps_double; + + for (int j = 1; j < steps; ++j) + { + robot_nav_2d_msgs::Pose2DStamped pose; + pose.header = last_stp.header; + pose.pose.x = last.x + j * delta_x; + pose.pose.y = last.y + j * delta_y; + pose.pose.theta = last.theta + j * delta_t; + global_plan_out.poses.push_back(pose); + } + } + global_plan_out.poses.push_back(global_plan_in.poses[i]); + last.x = loop.x; + last.y = loop.y; + } + return global_plan_out; + } + + using PoseList = std::vector; + + /** + * @brief Helper function for other version of compressPlan. + * + * Uses the Ramer Douglas Peucker algorithm. Ignores theta values + * + * @param input Full list of poses + * @param start_index Index of first pose (inclusive) + * @param end_index Index of last pose (inclusive) + * @param epsilon maximum allowable error. Increase for greater compression. + * @param list of poses possibly compressed for the poses[start_index, end_index] + */ + PoseList compressPlan(const PoseList &input, unsigned int start_index, unsigned int end_index, double epsilon) + { + // Skip if only two points + if (end_index - start_index <= 1) + { + PoseList::const_iterator first = input.begin() + start_index; + PoseList::const_iterator last = input.begin() + end_index + 1; + return PoseList(first, last); + } + + // Find the point with the maximum distance to the line from start to end + const robot_nav_2d_msgs::Pose2DStamped &start = input[start_index], + end = input[end_index]; + double max_distance = 0.0; + unsigned int max_index = 0; + for (unsigned int i = start_index + 1; i < end_index; i++) + { + const robot_nav_2d_msgs::Pose2DStamped &pose = input[i]; + double d = distanceToLine(pose.pose.x, pose.pose.y, start.pose.x, start.pose.y, end.pose.x, end.pose.y); + if (d > max_distance) + { + max_index = i; + max_distance = d; + } + } + + // If max distance is less than epsilon, eliminate all the points between start and end + if (max_distance <= epsilon) + { + PoseList outer; + outer.push_back(start); + outer.push_back(end); + return outer; + } + + // If max distance is greater than epsilon, recursively simplify + PoseList first_part = compressPlan(input, start_index, max_index, epsilon); + PoseList second_part = compressPlan(input, max_index, end_index, epsilon); + first_part.insert(first_part.end(), second_part.begin() + 1, second_part.end()); + return first_part; + } + + robot_nav_2d_msgs::Path2D compressPlan(const robot_nav_2d_msgs::Path2D &input_path, double epsilon) + { + robot_nav_2d_msgs::Path2D results; + results.header = input_path.header; + results.poses = compressPlan(input_path.poses, 0, input_path.poses.size() - 1, epsilon); + return results; + } + + void addPose(robot_nav_2d_msgs::Path2D &path, double x, double y, double theta) + { + robot_nav_2d_msgs::Pose2DStamped pose; + pose.pose.x = x; + pose.pose.y = y; + pose.pose.theta = theta; + path.poses.push_back(pose); + } +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/polygons.cpp b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp similarity index 86% rename from src/Libraries/nav_2d_utils/src/polygons.cpp rename to src/Libraries/robot_nav_2d_utils/src/polygons.cpp index ef8dd11..a86e048 100755 --- a/src/Libraries/nav_2d_utils/src/polygons.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/polygons.cpp @@ -32,8 +32,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include +#include #include #include #include @@ -41,11 +41,11 @@ #include #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { -using nav_2d_msgs::Point2D; -using nav_2d_msgs::Polygon2D; +using robot_nav_2d_msgs::Point2D; +using robot_nav_2d_msgs::Polygon2D; std::vector > parseVVD(const std::string& input) { @@ -285,7 +285,7 @@ XmlRpc::XmlRpcValue vectorToXMLRPC(const std::vector& array) return xml; } -XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays) +XmlRpc::XmlRpcValue polygonToXMLRPC(const robot_nav_2d_msgs::Polygon2D& polygon, bool array_of_arrays) { XmlRpc::XmlRpcValue xml; if (array_of_arrays) @@ -309,13 +309,13 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool return xml; } -// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, +// void polygonToParams(const robot_nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name, // bool array_of_arrays) // { // nh.setParam(parameter_name, polygonToXMLRPC(polygon, array_of_arrays)); // } -nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys) +robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, const std::vector& ys) { if (xs.size() < 3) { @@ -337,7 +337,7 @@ nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector& xs, return polygon; } -void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys) +void polygonToParallelArrays(const robot_nav_2d_msgs::Polygon2D polygon, std::vector& xs, std::vector& ys) { xs.clear(); ys.clear(); @@ -348,7 +348,7 @@ void polygonToParallelArrays(const nav_2d_msgs::Polygon2D polygon, std::vector 0; } -void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist) +void calculateMinAndMaxDistances(const robot_nav_2d_msgs::Polygon2D& polygon, double& min_dist, double& max_dist) { min_dist = std::numeric_limits::max(); max_dist = 0.0; @@ -429,7 +429,7 @@ void calculateMinAndMaxDistances(const nav_2d_msgs::Polygon2D& polygon, double& min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist)); } -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils // Adapt Point2D for use with the triangulation library namespace mapbox @@ -437,18 +437,18 @@ namespace mapbox namespace util { template <> -struct nth<0, nav_2d_msgs::Point2D> +struct nth<0, robot_nav_2d_msgs::Point2D> { - inline static double get(const nav_2d_msgs::Point2D& point) + inline static double get(const robot_nav_2d_msgs::Point2D& point) { return point.x; }; }; template <> -struct nth<1, nav_2d_msgs::Point2D> +struct nth<1, robot_nav_2d_msgs::Point2D> { - inline static double get(const nav_2d_msgs::Point2D& point) + inline static double get(const robot_nav_2d_msgs::Point2D& point) { return point.y; }; @@ -458,32 +458,32 @@ struct nth<1, nav_2d_msgs::Point2D> } // namespace mapbox -namespace nav_2d_utils +namespace robot_nav_2d_utils { -std::vector triangulate(const nav_2d_msgs::ComplexPolygon2D& polygon) +std::vector triangulate(const robot_nav_2d_msgs::ComplexPolygon2D& polygon) { // Compute the triangulation - std::vector> rings; + std::vector> rings; rings.reserve(1 + polygon.inner.size()); rings.push_back(polygon.outer.points); - for (const nav_2d_msgs::Polygon2D& inner : polygon.inner) + for (const robot_nav_2d_msgs::Polygon2D& inner : polygon.inner) { rings.push_back(inner.points); } std::vector indices = mapbox::earcut(rings); // Create a sequential point index. The triangulation results are indices in this vector. - std::vector points; + std::vector points; for (const auto& ring : rings) { - for (const nav_2d_msgs::Point2D& point : ring) + for (const robot_nav_2d_msgs::Point2D& point : ring) { points.push_back(point); } } // Construct the output triangles - std::vector result; + std::vector result; result.reserve(indices.size()); for (unsigned int index : indices) { @@ -492,12 +492,12 @@ std::vector triangulate(const nav_2d_msgs::ComplexPolygon2 return result; } -std::vector triangulate(const nav_2d_msgs::Polygon2D& polygon) +std::vector triangulate(const robot_nav_2d_msgs::Polygon2D& polygon) { - nav_2d_msgs::ComplexPolygon2D complex; + robot_nav_2d_msgs::ComplexPolygon2D complex; complex.outer = polygon; return triangulate(complex); } -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/src/tf_help.cpp b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp similarity index 71% rename from src/Libraries/nav_2d_utils/src/tf_help.cpp rename to src/Libraries/robot_nav_2d_utils/src/tf_help.cpp index dc997b6..00254cb 100755 --- a/src/Libraries/nav_2d_utils/src/tf_help.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/tf_help.cpp @@ -32,17 +32,17 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ - -#include -#include -#include -// #include +#include +#include +#include +#include +#include #include -namespace nav_2d_utils +namespace robot_nav_2d_utils { - bool transformPose(const TFListenerPtr tf, const std::string frame, - const geometry_msgs::PoseStamped &in_pose, geometry_msgs::PoseStamped &out_pose, + bool transformPose(const TFListenerPtr tf, const ::std::string frame, + const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose, const bool extrapolation_fallback) { // if (in_pose.header.frame_id == frame) @@ -56,7 +56,7 @@ namespace nav_2d_utils // tf->transform(in_pose, out_pose, frame); // return true; // } - // catch (tf::ExtrapolationException &ex) + // catch (tf3::ExtrapolationException &ex) // { // if (!extrapolation_fallback) // throw; @@ -66,16 +66,16 @@ namespace nav_2d_utils // tf->transform(latest_in_pose, out_pose, frame); // return true; // } - // catch (tf::TransformException &ex) + // catch (tf3::TransformException &ex) // { - // ROS_ERROR("Exception in transformPose: %s", ex.what()); + // robot::log_error("Exception in transformPose: %s", ex.what()); // return false; // } return false; } - bool transformPose(const TFListenerPtr tf, const std::string frame, - const nav_2d_msgs::Pose2DStamped &in_pose, nav_2d_msgs::Pose2DStamped &out_pose, + bool transformPose(const TFListenerPtr tf, const ::std::string frame, + const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose, const bool extrapolation_fallback) { geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose); @@ -89,12 +89,12 @@ namespace nav_2d_utils return ret; } - geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const nav_2d_msgs::Pose2DStamped &pose, - const std::string &frame_id) + ::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose, + const ::std::string &frame_id) { - nav_2d_msgs::Pose2DStamped local_pose; - nav_2d_utils::transformPose(tf, frame_id, pose, local_pose); + robot_nav_2d_msgs::Pose2DStamped local_pose; + robot_nav_2d_utils::transformPose(tf, frame_id, pose, local_pose); return local_pose.pose; } -} // namespace nav_2d_utils +} // namespace robot_nav_2d_utils diff --git a/src/Libraries/nav_2d_utils/test/bounds_test.cpp b/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp similarity index 96% rename from src/Libraries/nav_2d_utils/test/bounds_test.cpp rename to src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp index 58f840b..60755ed 100755 --- a/src/Libraries/nav_2d_utils/test/bounds_test.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp @@ -32,11 +32,11 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include #include -using nav_2d_utils::divideBounds; +using robot_nav_2d_utils::divideBounds; using nav_core2::UIntBounds; /** @@ -111,7 +111,7 @@ TEST(DivideBounds, iterative_tests) for (info.height = 1; info.height < 15; info.height++) { full_grid.setInfo(info); - UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info); + UIntBounds full_bounds = robot_nav_2d_utils::getFullUIntBounds(info); for (unsigned int rows = 1; rows < 11u; rows++) { for (unsigned int cols = 1; cols < 11u; cols++) @@ -156,7 +156,7 @@ TEST(DivideBounds, recursive_tests) info.height = 100; full_grid.setInfo(info); - UIntBounds full_bounds = nav_2d_utils::getFullUIntBounds(info); + UIntBounds full_bounds = robot_nav_2d_utils::getFullUIntBounds(info); std::vector level_one = divideBounds(full_bounds, 2, 2); ASSERT_EQ(level_one.size(), 4u); diff --git a/src/Libraries/nav_2d_utils/test/compress_test.cpp b/src/Libraries/robot_nav_2d_utils/test/compress_test.cpp similarity index 95% rename from src/Libraries/nav_2d_utils/test/compress_test.cpp rename to src/Libraries/robot_nav_2d_utils/test/compress_test.cpp index 41d5760..3326aac 100755 --- a/src/Libraries/nav_2d_utils/test/compress_test.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/compress_test.cpp @@ -32,14 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include -using nav_2d_utils::compressPlan; -using nav_2d_utils::addPose; +using robot_nav_2d_utils::compressPlan; +using robot_nav_2d_utils::addPose; TEST(CompressTest, compress_test) { - nav_2d_msgs::Path2D path; + robot_nav_2d_msgs::Path2D path; // Dataset borrowed from https://karthaus.nl/rdp/ addPose(path, 24, 173); addPose(path, 26, 170); diff --git a/src/Libraries/nav_2d_utils/test/param_tests.cpp b/src/Libraries/robot_nav_2d_utils/test/param_tests.cpp similarity index 72% rename from src/Libraries/nav_2d_utils/test/param_tests.cpp rename to src/Libraries/robot_nav_2d_utils/test/param_tests.cpp index 00c90cd..0e60bfb 100755 --- a/src/Libraries/nav_2d_utils/test/param_tests.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/param_tests.cpp @@ -34,14 +34,14 @@ * POSSIBILITY OF SUCH DAMAGE. * * Author: Dave Hershberger -* David V. Lu!! (nav_2d_utils version) +* David V. Lu!! (robot_nav_2d_utils version) *********************************************************************/ #include #include -#include +#include -using nav_2d_utils::polygonFromParams; -using nav_2d_msgs::Polygon2D; +using robot_nav_2d_utils::polygonFromParams; +using robot_nav_2d_msgs::Polygon2D; TEST(Polygon2D, unpadded_footprint_from_string_param) { @@ -64,7 +64,7 @@ TEST(Polygon2D, check_search_capabilities) YAML::Node nh("~unpadded/unneccessarily/long_namespace"); Polygon2D footprint = polygonFromParams(nh, "footprint"); ASSERT_EQ(3U, footprint.points.size()); - EXPECT_THROW(polygonFromParams(nh, "footprint", false), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint", false), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, footprint_from_xmlrpc_param) @@ -86,7 +86,7 @@ TEST(Polygon2D, footprint_from_xmlrpc_param) EXPECT_FLOAT_EQ(-0.1f, footprint.points[ 3 ].y); Polygon2D footprint2 = polygonFromParams(nh, "footprint2"); - ASSERT_TRUE(nav_2d_utils::equals(footprint, footprint2)); + ASSERT_TRUE(robot_nav_2d_utils::equals(footprint, footprint2)); } TEST(Polygon2D, footprint_from_same_level_param) @@ -108,34 +108,34 @@ TEST(Polygon2D, footprint_from_same_level_param) TEST(Polygon2D, footprint_from_xmlrpc_param_failure) { YAML::Node nh("~xmlrpc_fail"); - EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint2"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint3"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint4"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint5"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint6"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint7"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint8"), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParams(nh, "footprint9"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint2"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint3"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint4"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint5"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint6"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint7"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint8"), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint9"), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, footprint_empty) { YAML::Node nh("~empty"); - EXPECT_THROW(polygonFromParams(nh, "footprint"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParams(nh, "footprint"), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, test_write) { YAML::Node nh("~unpadded"); Polygon2D footprint = polygonFromParams(nh, "footprint"); - nh.setParam("another_footprint", nav_2d_utils::polygonToXMLRPC(footprint)); + nh.setParam("another_footprint", robot_nav_2d_utils::polygonToXMLRPC(footprint)); Polygon2D another_footprint = polygonFromParams(nh, "another_footprint"); - EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint)); + EXPECT_TRUE(robot_nav_2d_utils::equals(footprint, another_footprint)); - nh.setParam("third_footprint", nav_2d_utils::polygonToXMLRPC(footprint, false)); + nh.setParam("third_footprint", robot_nav_2d_utils::polygonToXMLRPC(footprint, false)); another_footprint = polygonFromParams(nh, "third_footprint"); - EXPECT_TRUE(nav_2d_utils::equals(footprint, another_footprint)); + EXPECT_TRUE(robot_nav_2d_utils::equals(footprint, another_footprint)); } int main(int argc, char** argv) diff --git a/src/Libraries/nav_2d_utils/test/param_tests.launch b/src/Libraries/robot_nav_2d_utils/test/param_tests.launch similarity index 91% rename from src/Libraries/nav_2d_utils/test/param_tests.launch rename to src/Libraries/robot_nav_2d_utils/test/param_tests.launch index ead5813..d4e0730 100755 --- a/src/Libraries/nav_2d_utils/test/param_tests.launch +++ b/src/Libraries/robot_nav_2d_utils/test/param_tests.launch @@ -1,5 +1,5 @@ - + diff --git a/src/Libraries/nav_2d_utils/test/polygon_tests.cpp b/src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp similarity index 74% rename from src/Libraries/nav_2d_utils/test/polygon_tests.cpp rename to src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp index 9bbc220..b3bcf5a 100755 --- a/src/Libraries/nav_2d_utils/test/polygon_tests.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include -using nav_2d_utils::parseVVD; -using nav_2d_msgs::Polygon2D; -using nav_2d_utils::polygonFromString; -using nav_2d_utils::polygonFromParallelArrays; +using robot_nav_2d_utils::parseVVD; +using robot_nav_2d_msgs::Polygon2D; +using robot_nav_2d_utils::polygonFromString; +using robot_nav_2d_utils::polygonFromParallelArrays; TEST(array_parser, basic_operation) { @@ -55,22 +55,22 @@ TEST(array_parser, basic_operation) TEST(array_parser, missing_open) { - EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]]"), robot_nav_2d_utils::PolygonParseException); } TEST(array_parser, missing_close) { - EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(parseVVD("[[1, 2.2], [.3, -4e4]"), robot_nav_2d_utils::PolygonParseException); } TEST(array_parser, wrong_depth) { - EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(parseVVD("[1, 2.2], [.3, -4e4]"), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, radius_param) { - Polygon2D footprint = nav_2d_utils::polygonFromRadius(10.0); + Polygon2D footprint = robot_nav_2d_utils::polygonFromRadius(10.0); // Circular robot has 16-point footprint auto-generated. ASSERT_EQ(16U, footprint.points.size()); @@ -101,22 +101,22 @@ TEST(Polygon2D, string_param) TEST(Polygon2D, broken_string_param) { // Not enough points - EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString("[[1, 1], [-1, 1]]"), robot_nav_2d_utils::PolygonParseException); // Too many numbers in point - EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString("[[1, 1, 1], [-1, 1], [-1, -1]]"), robot_nav_2d_utils::PolygonParseException); // Unexpected character - EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString("[[x, 1], [-1, 1], [-1, -1]]"), robot_nav_2d_utils::PolygonParseException); // Empty String - EXPECT_THROW(polygonFromString(""), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString(""), robot_nav_2d_utils::PolygonParseException); // Empty List - EXPECT_THROW(polygonFromString("[]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString("[]"), robot_nav_2d_utils::PolygonParseException); // Empty Point - EXPECT_THROW(polygonFromString("[[]]"), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromString("[[]]"), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, arrays) @@ -141,20 +141,20 @@ TEST(Polygon2D, broken_arrays) std::vector shorty = {1, -1}; std::vector three = {1, 1, -1}; std::vector four = {1, 1, -1, -1}; - EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), nav_2d_utils::PolygonParseException); - EXPECT_THROW(polygonFromParallelArrays(three, four), nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParallelArrays(shorty, shorty), robot_nav_2d_utils::PolygonParseException); + EXPECT_THROW(polygonFromParallelArrays(three, four), robot_nav_2d_utils::PolygonParseException); } TEST(Polygon2D, test_move) { Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"); geometry_msgs::Pose2D pose; - Polygon2D square2 = nav_2d_utils::movePolygonToPose(square, pose); - EXPECT_TRUE(nav_2d_utils::equals(square, square2)); + Polygon2D square2 = robot_nav_2d_utils::movePolygonToPose(square, pose); + EXPECT_TRUE(robot_nav_2d_utils::equals(square, square2)); pose.x = 15; pose.y = -10; pose.theta = M_PI / 4; - Polygon2D diamond = nav_2d_utils::movePolygonToPose(square, pose); + Polygon2D diamond = robot_nav_2d_utils::movePolygonToPose(square, pose); ASSERT_EQ(4U, diamond.points.size()); double side = 1.0 / sqrt(2); @@ -171,11 +171,11 @@ TEST(Polygon2D, test_move) TEST(Polygon2D, inside) { Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]"); - EXPECT_TRUE(nav_2d_utils::isInside(square, 0.00, 0.00)); - EXPECT_TRUE(nav_2d_utils::isInside(square, 0.45, 0.45)); - EXPECT_FALSE(nav_2d_utils::isInside(square, 0.50, 0.50)); - EXPECT_FALSE(nav_2d_utils::isInside(square, 0.00, 0.50)); - EXPECT_FALSE(nav_2d_utils::isInside(square, 0.55, 0.55)); + EXPECT_TRUE(robot_nav_2d_utils::isInside(square, 0.00, 0.00)); + EXPECT_TRUE(robot_nav_2d_utils::isInside(square, 0.45, 0.45)); + EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.50, 0.50)); + EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.00, 0.50)); + EXPECT_FALSE(robot_nav_2d_utils::isInside(square, 0.55, 0.55)); } int main(int argc, char** argv) diff --git a/src/Libraries/nav_2d_utils/test/resolution_test.cpp b/src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp similarity index 93% rename from src/Libraries/nav_2d_utils/test/resolution_test.cpp rename to src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp index 1a676b5..7e03a9a 100755 --- a/src/Libraries/nav_2d_utils/test/resolution_test.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp @@ -32,14 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include -using nav_2d_utils::adjustPlanResolution; -using nav_2d_utils::addPose; +using robot_nav_2d_utils::adjustPlanResolution; +using robot_nav_2d_utils::addPose; TEST(ResolutionTest, simple_example) { - nav_2d_msgs::Path2D path; + robot_nav_2d_msgs::Path2D path; // Space between points is one meter addPose(path, 0.0, 0.0); addPose(path, 0.0, 1.0); @@ -62,7 +62,7 @@ TEST(ResolutionTest, simple_example) TEST(ResolutionTest, real_example) { // This test is based on a real-world example - nav_2d_msgs::Path2D path; + robot_nav_2d_msgs::Path2D path; addPose(path, 17.779193, -0.972024); addPose(path, 17.799171, -0.950775); addPose(path, 17.851942, -0.903709);