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

changed name robot_nav_2d_utils robot_nav_2d_msgs

Browse Source
This commit is contained in:
HiepLM 2025-12-29 11:38:15 +07:00
parent 6b327a523e
commit 307a9c84f9
132 changed files with 3540 additions and 3543 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
CMakeLists.txt
View File

@ -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")

2
doc/architecture_discussion.md
View File

@ -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:** ⚠️

2
doc/implementation_plan.md
View File

@ -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:

2
setup_catkin_workspace.sh
View File

@ -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"

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

@ -449,8 +449,9 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou
try {
move_base_core::BaseNavigation* nav = static_cast<move_base_core::BaseNavigation*>(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;

2
src/Algorithms/Cores/score_algorithm/CMakeLists.txt
View File

@ -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)

10
src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h
View File

@ -5,9 +5,9 @@
#include <robot/console.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <memory>
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

26
src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
View File

@ -3,9 +3,9 @@
#include <robot/node_handle.h>
#include <iostream>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/common.h>
#include <nav_core2/exceptions.h>
@ -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<nav_2d_msgs::Pose2DStamped> &plan,
virtual unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &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<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes);
virtual bool findSubGoalIndex(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<robot_nav_2d_msgs::Pose2DStamped> &mutes);
/**
* @brief Calculate journey
@ -129,7 +129,7 @@ namespace score_algorithm
* @param last_valid_index
* @return journey
*/
double journey(const std::vector<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
double journey(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &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<nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
std::vector<robot_nav_2d_msgs::Pose2DStamped> reached_intermediate_goals_;
std::vector<unsigned int> start_index_saved_vt_;
unsigned int sub_goal_index_saved_, sub_goal_seq_saved_;
unsigned int sub_start_index_saved_, sub_start_seq_saved_;

22
src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h
View File

@ -3,9 +3,9 @@
#include <robot/node_handle.h>
#include <mkt_msgs/Trajectory2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <memory>
#include <vector>
#include <geometry_msgs/Vector3.h>
@ -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 &current_velocity) = 0;
virtual void startNewIteration(const robot_nav_2d_msgs::Twist2D &current_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<nav_2d_msgs::Twist2D> getTwists(const nav_2d_msgs::Twist2D &current_velocity)
virtual std::vector<robot_nav_2d_msgs::Twist2D> getTwists(const robot_nav_2d_msgs::Twist2D &current_velocity)
{
std::vector<nav_2d_msgs::Twist2D> twists;
std::vector<robot_nav_2d_msgs::Twist2D> 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;

44
src/Algorithms/Cores/score_algorithm/src/score_algorithm.cpp
View File

@ -1,9 +1,9 @@
#include <score_algorithm/score_algorithm.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/conversions.h>
#include <angles/angles.h>
unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<nav_2d_msgs::Pose2DStamped> &plan,
unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<robot_nav_2d_msgs::Pose2DStamped> &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<nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<nav_2d_msgs::Pose2DStamped> &mutes)
bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &plan, std::vector<unsigned int> &seq_s, std::vector<robot_nav_2d_msgs::Pose2DStamped> &mutes)
{
if (plan.empty())
{
@ -50,7 +50,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_
poseArrayMsg.header.stamp = robot::Time::now();
poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
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<nav_2d_
{
seq_s.push_back(plan[i].header.seq);
mutes.push_back(plan[i]);
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
poseArrayMsg.poses.push_back(pose);
}
}
@ -103,7 +103,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_
{
seq_s.push_back(plan[i + 1].header.seq);
mutes.push_back(plan[i + 1]);
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
poseArrayMsg.poses.push_back(pose);
}
p2 = p3;
@ -115,7 +115,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
else
@ -124,15 +124,15 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<nav_2d_
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
}
return true;
}
bool score_algorithm::ScoreAlgorithm::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)
bool score_algorithm::ScoreAlgorithm::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)
{
result.poses.clear();
if (global_plan.poses.empty())
@ -142,7 +142,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const nav_2d_msgs::Pose
}
std::vector<unsigned int> seq_s;
std::vector<nav_2d_msgs::Pose2DStamped> mutes;
std::vector<robot_nav_2d_msgs::Pose2DStamped> 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<nav_2d_msgs::Pose2DStamped> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
double score_algorithm::ScoreAlgorithm::journey(const std::vector<robot_nav_2d_msgs::Pose2DStamped> &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;
}

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

@ -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

12
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h
View File

@ -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<geometry_msgs::Pose2D> &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<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
@ -77,7 +77,7 @@ protected:
std::vector<geometry_msgs::Pose2D> 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_;

6
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h
View File

@ -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:

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/goal_checker.h
View File

@ -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<geometry_msgs::PoseStamped>& path, const nav_2d_msgs::Twist2D& velocity) override;
const std::vector<geometry_msgs::PoseStamped>& path, const robot_nav_2d_msgs::Twist2D& velocity) override;
protected:

6
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h
View File

@ -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:

6
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h
View File

@ -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:

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_go_straight.h
View File

@ -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

58
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@ -7,7 +7,7 @@
#include <boost/dll/alias.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
@ -18,10 +18,10 @@
#include <kalman/kalman.h>
#include <vector>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/conversions.h>
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<nav_2d_msgs::Pose2DStamped>::iterator
getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan);
std::vector<robot_nav_2d_msgs::Pose2DStamped>::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<geometry_msgs::Point> interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> 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<double>& 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<double> 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_;

6
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_rotate_to_goal.h
View File

@ -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

12
src/Algorithms/Libraries/mkt_algorithm/package.xml
View File

@ -51,10 +51,10 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>score_algorithm</build_depend>
<build_depend>nav_2d_msgs</build_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_2d_utils</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>kalman</build_depend>
<build_depend>ddynamic_reconfigure</build_depend>
<build_depend>costmap_2d</build_depend>
@ -62,10 +62,10 @@
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>score_algorithm</build_export_depend>
<build_export_depend>nav_2d_msgs</build_export_depend>
<build_export_depend>robot_nav_2d_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>nav_2d_utils</build_export_depend>
<build_export_depend>robot_nav_2d_utils</build_export_depend>
<build_export_depend>kalman</build_export_depend>
<build_export_depend>ddynamic_reconfigure</build_export_depend>
<build_export_depend>costmap_2d</build_export_depend>
@ -73,10 +73,10 @@
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>score_algorithm</exec_depend>
<exec_depend>nav_2d_msgs</exec_depend>
<exec_depend>robot_nav_2d_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>nav_2d_utils</exec_depend>
<exec_depend>robot_nav_2d_utils</exec_depend>
<exec_depend>kalman</exec_depend>
<exec_depend>ddynamic_reconfigure</exec_depend>
<exec_depend>costmap_2d</exec_depend>

14
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp
View File

@ -1,5 +1,5 @@
#include <mkt_algorithm/go_straight.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
@ -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;

60
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp
View File

@ -1,10 +1,10 @@
#include <mkt_algorithm/bicycle.h>
#include <pluginlib/class_list_macros.h>
#include <nav_grid/coordinate_conversion.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <angles/angles.h>
@ -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<geometry_msgs::Pose2D>'
// Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<geometry_msgs::Pose2D>'
std::vector<geometry_msgs::Pose2D> plan = global_plan.poses;
std::vector<unsigned int> 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<geometry_msgs::Pose2D> &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<geometry_msgs::Pose2D> &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;

14
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp
View File

@ -1,5 +1,5 @@
#include <mkt_algorithm/rotate_to_goal.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <angles/angles.h>
@ -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;

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

@ -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);

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

@ -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<nav_2d_msgs::Pose2DStamped>::iterator
mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const nav_2d_msgs::Twist2D &velocity, const double &lookahead_dist, nav_2d_msgs::Path2D global_plan)
std::vector<robot_nav_2d_msgs::Pose2DStamped>::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<nav_2d_msgs::Pose2DStamped>::iterator it = global_plan.poses.begin();
std::vector<nav_2d_msgs::Pose2DStamped>::iterator erase_end = it;
std::vector<robot_nav_2d_msgs::Pose2DStamped>::iterator it = global_plan.poses.begin();
std::vector<robot_nav_2d_msgs::Pose2DStamped>::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;

14
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp
View File

@ -1,6 +1,6 @@
#include <robot/console.h>
#include <mkt_algorithm/diff/diff_rotate_to_goal.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <angles/angles.h>
@ -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<bool>("avoid_obstacles", avoid_obstacles_, false);
nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param<double>("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;

2
src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt
View File

@ -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

4
src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h
View File

@ -8,7 +8,7 @@
#include <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <geometry_msgs/Pose2D.h>
namespace mkt_msgs
@ -28,7 +28,7 @@ namespace mkt_msgs
(void)_alloc;
}
typedef ::nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
typedef ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef std::vector<geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<geometry_msgs::Pose2D>> _poses_type;

4
src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt
View File

@ -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

4
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/goal_checker.h
View File

@ -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

2
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/limited_accel_generator.h
View File

@ -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

4
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h
View File

@ -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

20
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h
View File

@ -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 &current_velocity) override;
void startNewIteration(const robot_nav_2d_msgs::Twist2D &current_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<double> getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel);
virtual std::vector<double> getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel);
robot::NodeHandle nh_kinematics_;
KinematicParameters::Ptr kinematics_;

6
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h
View File

@ -2,7 +2,7 @@
#define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_
#include <robot/node_handle.h>
#include <nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <mkt_plugins/kinematic_parameters.h>
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

4
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/xy_theta_iterator.h
View File

@ -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:
/**

14
src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp
View File

@ -1,9 +1,9 @@
#include <robot/console.h>
#include <mkt_plugins/goal_checker.h>
#include <boost/dll/alias.hpp>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <angles/angles.h>
#include <cmath>
@ -23,11 +23,11 @@ void mkt_plugins::GoalChecker::intParam(const robot::NodeHandle &nh)
{
if (!nh.param<double>("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<double>("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;

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

@ -1,11 +1,11 @@
#include <mkt_plugins/kinematic_parameters.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <memory>
#include <string>
#include <cmath>
using nav_2d_utils::moveDeprecatedParameter;
using robot_nav_2d_utils::moveDeprecatedParameter;
namespace mkt_plugins
{

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

@ -1,6 +1,6 @@
#include <robot/console.h>
#include <mkt_plugins/limited_accel_generator.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <nav_core2/exceptions.h>
#include <vector>
@ -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_);

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

@ -1,7 +1,7 @@
#include <mkt_plugins/simple_goal_checker.h>
#include <boost/dll/alias.hpp>
#include <angles/angles.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <cmath>
namespace mkt_plugins
@ -23,15 +23,15 @@ void SimpleGoalChecker::intParam(const robot::NodeHandle& nh)
{
if(!nh.param<double>("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<double>("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<bool>("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_)

28
src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
View File

@ -1,6 +1,6 @@
#include <mkt_plugins/standard_traj_generator.h>
#include <mkt_plugins/xy_theta_iterator.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <nav_core2/exceptions.h>
#include <algorithm>
@ -9,7 +9,7 @@
#include <string>
#include <vector>
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 &current_velocity)
void StandardTrajectoryGenerator::startNewIteration(const robot_nav_2d_msgs::Twist2D &current_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<double> StandardTrajectoryGenerator::getTimeSteps(const nav_2d_msgs::Twist2D &cmd_vel)
std::vector<double> StandardTrajectoryGenerator::getTimeSteps(const robot_nav_2d_msgs::Twist2D &cmd_vel)
{
std::vector<double> 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<double> 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;

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

@ -1,5 +1,5 @@
#include <mkt_plugins/xy_theta_iterator.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/parameters.h>
#include <memory>
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<OneDVelocityIterator>(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();

2
src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
View File

@ -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

20
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
View File

@ -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<DockingPlanner*> 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

10
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
View File

@ -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_;
};

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

@ -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<score_algorithm::TrajectoryGenerator::Ptr()> 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_;

10
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h
View File

@ -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;
};
}

12
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h
View File

@ -2,8 +2,8 @@
#define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__
#include <robot/node_handle.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_utils/conversions.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_utils/conversions.h>
#include <nav_core2/common.h>
#include <nav_core2/exceptions.h>
@ -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);
}

74
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@ -1,10 +1,10 @@
#include <pnkx_local_planner/pnkx_docking_local_planner.h>
#include <nav_core2/exceptions.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
#include <nav_2d_utils/footprint.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/footprint.h>
#include <angles/angles.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Bool.h>
@ -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<double>("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<double>("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<geometry_msgs::PoseStamped> 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;
}
}

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

@ -1,8 +1,8 @@
#include <nav_core2/exceptions.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <mkt_msgs/Trajectory2D.h>
// #include <g2o/core/base_binary_edge.h>
@ -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 == "")
{

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

@ -1,10 +1,10 @@
#include <pnkx_local_planner/pnkx_local_planner.h>
#include <nav_core2/exceptions.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <angles/angles.h>
#include <tf3/convert.h>
@ -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<double>("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<double>("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;
}

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

@ -2,10 +2,10 @@
#include "pnkx_local_planner/transforms.h"
#include <nav_core2/exceptions.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/parameters.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/parameters.h>
#include <mkt_msgs/Trajectory2D.h>
#include <angles/angles.h>
@ -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);

32
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
View File

@ -1,11 +1,11 @@
#include <pnkx_local_planner/transforms.h>
#include <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot/console.h>
#include <data_convert/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
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<nav_2d_msgs::Pose2DStamped>::iterator it = transformed_plan.poses.begin();
std::vector<robot_nav_2d_msgs::Pose2DStamped>::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)
{

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/ComplexPolygon2D.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Polygon2D.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct ComplexPolygon2D_
{
typedef ComplexPolygon2D_<ContainerAllocator> Type;
ComplexPolygon2D_()
: outer()
, inner() {
}
ComplexPolygon2D_(const ContainerAllocator& _alloc)
: outer(_alloc)
, inner(_alloc) {
(void)_alloc;
}
typedef ::nav_2d_msgs::Polygon2D_<ContainerAllocator> _outer_type;
_outer_type outer;
typedef std::vector< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> >> _inner_type;
_inner_type inner;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct ComplexPolygon2D_
typedef ::nav_2d_msgs::ComplexPolygon2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return lhs.outer == rhs.outer &&
lhs.inner == rhs.inner;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfChars.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/NavGridInfo.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfChars_
{
typedef NavGridOfChars_<ContainerAllocator> 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_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfChars_
typedef ::nav_2d_msgs::NavGridOfChars_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfCharsUpdate.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/UIntBounds.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfCharsUpdate_
{
typedef NavGridOfCharsUpdate_<ContainerAllocator> 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_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfCharsUpdate_
typedef ::nav_2d_msgs::NavGridOfCharsUpdate_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoubles.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/NavGridInfo.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoubles_
{
typedef NavGridOfDoubles_<ContainerAllocator> 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_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoubles_
typedef ::nav_2d_msgs::NavGridOfDoubles_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H

80
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridOfDoublesUpdate.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <nav_2d_msgs/UIntBounds.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoublesUpdate_
{
typedef NavGridOfDoublesUpdate_<ContainerAllocator> 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_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoublesUpdate_
typedef ::nav_2d_msgs::NavGridOfDoublesUpdate_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Path2D.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/Pose2DStamped.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Path2D_
{
typedef Path2D_<ContainerAllocator> 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_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;
_poses_type poses;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Path2D_<ContainerAllocator> const> ConstPtr;
}; // struct Path2D_
typedef ::nav_2d_msgs::Path2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.poses == rhs.poses;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_PATH2D_H

72
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Point2D.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Point2D_
{
typedef Point2D_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Point2D_<ContainerAllocator> const> ConstPtr;
}; // struct Point2D_
typedef ::nav_2d_msgs::Point2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POINT2D_H

67
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2D.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <nav_2d_msgs/Point2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2D_
{
typedef Polygon2D_<ContainerAllocator> Type;
Polygon2D_()
: points() {
}
Polygon2D_(const ContainerAllocator& _alloc)
: points(_alloc) {
(void)_alloc;
}
typedef std::vector< ::nav_2d_msgs::Point2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::Point2D_<ContainerAllocator> >> _points_type;
_points_type points;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2D_
typedef ::nav_2d_msgs::Polygon2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return lhs.points == rhs.points;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2D_H

81
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DCollection.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/ComplexPolygon2D.h>
#include <std_msgs/ColorRGBA.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DCollection_
{
typedef Polygon2DCollection_<ContainerAllocator> 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_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
_polygons_type polygons;
typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
_colors_type colors;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DCollection_
typedef ::nav_2d_msgs::Polygon2DCollection_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygons == rhs.polygons &&
lhs.colors == rhs.colors;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Polygon2DStamped.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DStamped_
{
typedef Polygon2DStamped_<ContainerAllocator> 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_<ContainerAllocator> _polygon_type;
_polygon_type polygon;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DStamped_
typedef ::nav_2d_msgs::Polygon2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygon == rhs.polygon;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H

78
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2D32.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Pose2D32_
{
typedef Pose2D32_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2D32_<ContainerAllocator> const> ConstPtr;
}; // struct Pose2D32_
typedef ::nav_2d_msgs::Pose2D32_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POSE2D32_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Pose2DStamped.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <geometry_msgs/Pose2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Pose2DStamped_
{
typedef Pose2DStamped_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Pose2DStamped_
typedef ::nav_2d_msgs::Pose2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.pose == rhs.pose;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H

30
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPlugin.h
View File

@ -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 <nav_2d_msgs/SwitchPluginRequest.h>
#include <nav_2d_msgs/SwitchPluginResponse.h>
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

66
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginRequest.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct SwitchPluginRequest_
{
typedef SwitchPluginRequest_<ContainerAllocator> Type;
SwitchPluginRequest_()
: new_plugin() {
}
SwitchPluginRequest_(const ContainerAllocator& _alloc)
: new_plugin(_alloc) {
(void)_alloc;
}
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _new_plugin_type;
_new_plugin_type new_plugin;
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> const> ConstPtr;
}; // struct SwitchPluginRequest_
typedef ::nav_2d_msgs::SwitchPluginRequest_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
{
return lhs.new_plugin == rhs.new_plugin;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H

72
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/SwitchPluginResponse.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct SwitchPluginResponse_
{
typedef SwitchPluginResponse_<ContainerAllocator> 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<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _message_type;
_message_type message;
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> const> ConstPtr;
}; // struct SwitchPluginResponse_
typedef ::nav_2d_msgs::SwitchPluginResponse_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
{
return lhs.success == rhs.success &&
lhs.message == rhs.message;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H

78
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2D_
{
typedef Twist2D_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2D_
typedef ::nav_2d_msgs::Twist2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_TWIST2D_H

78
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2D32.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2D32_
{
typedef Twist2D32_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Twist2D32_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2D32_
typedef ::nav_2d_msgs::Twist2D32_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_TWIST2D32_H

74
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/Twist2DStamped.h
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <nav_2d_msgs/Twist2D.h>
namespace nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2DStamped_
{
typedef Twist2DStamped_<ContainerAllocator> 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_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2DStamped_
typedef ::nav_2d_msgs::Twist2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.velocity == rhs.velocity;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace nav_2d_msgs
#endif // NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H

91
src/Libraries/nav_2d_utils/include/nav_2d_utils/bounds.h
View File

@ -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 <nav_grid/nav_grid_info.h>
#include <nav_core2/bounds.h>
#include <vector>
/**
* @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<nav_core2::UIntBounds> 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

106
src/Libraries/nav_2d_utils/include/nav_2d_utils/conversions.h
View File

@ -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 <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PolygonStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Point2D.h>
#include <nav_2d_msgs/Polygon2D.h>
#include <nav_2d_msgs/Polygon2DStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/NavGridInfo.h>
#include <nav_2d_msgs/UIntBounds.h>
#include <nav_msgs/MapMetaData.h>
#include <nav_msgs/Path.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/bounds.h>
// #include <tf/tf.h>
#include <vector>
#include <string>
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<tf::Pose>& 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<geometry_msgs::PoseStamped>& poses);
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 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

153
src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h
View File

@ -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 <robot/node_handle.h>
#include <robot/console.h>
#include <string>
#include <iostream>
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<class param_t>
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<class param_t>
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<class param_t>
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<param_t>(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<class param_t>
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

88
src/Libraries/nav_2d_utils/include/nav_2d_utils/path_ops.h
View File

@ -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 <nav_2d_msgs/Path2D.h>
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

274
src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h
View File

@ -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 <ros/ros.h>
#include <pluginlib/class_loader.h>
#include <std_msgs/String.h>
#include <nav_2d_msgs/SwitchPlugin.h>
#include <map>
#include <string>
#include <vector>
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 T>
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<std::string> getPluginNames() const
{
std::vector<std::string> 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<void(const std::string&, const std::string&)>;
/**
* @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<T> plugin_loader_;
std::map<std::string, boost::shared_ptr<T>> 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<class T>
PluginMux<T>::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<std_msgs::String>(ros_name_, 1, true);
// Load Plugins
std::string plugin_class_name;
std::vector<std::string> 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<class T>
void PluginMux<T>::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

91
src/Libraries/nav_2d_utils/include/nav_2d_utils/tf_help.h
View File

@ -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 <nav_core2/common.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <string>
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

102
src/Libraries/nav_2d_utils/src/bounds.cpp
View File

@ -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 <nav_2d_utils/bounds.h>
#include <nav_grid/coordinate_conversion.h>
#include <algorithm>
#include <stdexcept>
#include <vector>
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<nav_core2::UIntBounds> 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<unsigned int>(ceil(static_cast<double>(full_width) / n_cols)),
small_height = static_cast<unsigned int>(ceil(static_cast<double>(full_height) / n_rows));
std::vector<nav_core2::UIntBounds> 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

339
src/Libraries/nav_2d_utils/src/conversions.cpp
View File

@ -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 <nav_2d_utils/conversions.h>
#include <vector>
#include <string>
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<tf::Pose>& 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<geometry_msgs::PoseStamped>& 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<geometry_msgs::PoseStamped>& 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<geometry_msgs::Pose2D>& 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

193
src/Libraries/nav_2d_utils/src/path_ops.cpp
View File

@ -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 <nav_2d_utils/path_ops.h>
#include <nav_2d_utils/geometry_help.h>
#include <cmath>
#include <vector>
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<int>(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<nav_2d_msgs::Pose2DStamped>;
/**
* @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

24
src/Libraries/nav_2d_msgs/CMakeLists.txt → src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
View File

@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
@ -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 "=================================")

74
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot_nav_2d_msgs/Polygon2D.h>
#include <robot_nav_2d_msgs/Polygon2D.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct ComplexPolygon2D_
{
typedef ComplexPolygon2D_<ContainerAllocator> Type;
ComplexPolygon2D_()
: outer()
, inner() {
}
ComplexPolygon2D_(const ContainerAllocator& _alloc)
: outer(_alloc)
, inner(_alloc) {
(void)_alloc;
}
typedef ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> _outer_type;
_outer_type outer;
typedef std::vector< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> >> _inner_type;
_inner_type inner;
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct ComplexPolygon2D_
typedef ::robot_nav_2d_msgs::ComplexPolygon2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return lhs.outer == rhs.outer &&
lhs.inner == rhs.inner;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_COMPLEXPOLYGON2D_H

26
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/NavGridInfo.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridInfo.h
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace nav_2d_msgs
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridInfo_
@ -57,22 +57,22 @@ struct NavGridInfo_
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridInfo_
typedef ::nav_2d_msgs::NavGridInfo_<std::allocator<void> > NavGridInfo;
typedef ::robot_nav_2d_msgs::NavGridInfo_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
bool operator==(const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
{
return lhs.width == rhs.width &&
lhs.height == rhs.height &&
@ -83,12 +83,12 @@ bool operator==(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, co
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & rhs)
bool operator!=(const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridInfo_<ContainerAllocator2> & 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

80
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <robot_nav_2d_msgs/NavGridInfo.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfChars_
{
typedef NavGridOfChars_<ContainerAllocator> 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_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfChars_
typedef ::robot_nav_2d_msgs::NavGridOfChars_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfChars_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARS_H

80
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <robot_nav_2d_msgs/UIntBounds.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfCharsUpdate_
{
typedef NavGridOfCharsUpdate_<ContainerAllocator> 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_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<uint8_t, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<uint8_t>> _data_type;
_data_type data;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfCharsUpdate_
typedef ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfCharsUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFCHARSUPDATE_H

80
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <robot_nav_2d_msgs/NavGridInfo.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoubles_
{
typedef NavGridOfDoubles_<ContainerAllocator> 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_<ContainerAllocator> _info_type;
_info_type info;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoubles_
typedef ::robot_nav_2d_msgs::NavGridOfDoubles_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.info == rhs.info &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoubles_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLES_H

80
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot/time.h>
#include <robot_nav_2d_msgs/UIntBounds.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct NavGridOfDoublesUpdate_
{
typedef NavGridOfDoublesUpdate_<ContainerAllocator> 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_<ContainerAllocator> _bounds_type;
_bounds_type bounds;
typedef std::vector<double, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<double>> _data_type;
_data_type data;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator> const> ConstPtr;
}; // struct NavGridOfDoublesUpdate_
typedef ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return lhs.stamp == rhs.stamp &&
lhs.bounds == rhs.bounds &&
lhs.data == rhs.data;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::NavGridOfDoublesUpdate_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_NAVGRIDOFDOUBLESUPDATE_H

69
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Path2D_
{
typedef Path2D_<ContainerAllocator> 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_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> >> _poses_type;
_poses_type poses;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Path2D_<ContainerAllocator> const> ConstPtr;
}; // struct Path2D_
typedef ::robot_nav_2d_msgs::Path2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.poses == rhs.poses;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Path2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_PATH2D_H

72
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Point2D_
{
typedef Point2D_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> const> ConstPtr;
}; // struct Point2D_
typedef ::robot_nav_2d_msgs::Point2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Point2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POINT2D_H

67
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <robot_nav_2d_msgs/Point2D.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2D_
{
typedef Polygon2D_<ContainerAllocator> Type;
Polygon2D_()
: points() {
}
Polygon2D_(const ContainerAllocator& _alloc)
: points(_alloc) {
(void)_alloc;
}
typedef std::vector< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::Point2D_<ContainerAllocator> >> _points_type;
_points_type points;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2D_
typedef ::robot_nav_2d_msgs::Polygon2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return lhs.points == rhs.points;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2D_H

81
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <robot_nav_2d_msgs/ComplexPolygon2D.h>
#include <std_msgs/ColorRGBA.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DCollection_
{
typedef Polygon2DCollection_<ContainerAllocator> 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_<ContainerAllocator> , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::robot_nav_2d_msgs::ComplexPolygon2D_<ContainerAllocator> >> _polygons_type;
_polygons_type polygons;
typedef std::vector< ::std_msgs::ColorRGBA , typename std::allocator_traits<ContainerAllocator>::template rebind_alloc< ::std_msgs::ColorRGBA >> _colors_type;
_colors_type colors;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DCollection_
typedef ::robot_nav_2d_msgs::Polygon2DCollection_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygons == rhs.polygons &&
lhs.colors == rhs.colors;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DCollection_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DCOLLECTION_H

74
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <robot_nav_2d_msgs/Polygon2D.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Polygon2DStamped_
{
typedef Polygon2DStamped_<ContainerAllocator> 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_<ContainerAllocator> _polygon_type;
_polygon_type polygon;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Polygon2DStamped_
typedef ::robot_nav_2d_msgs::Polygon2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.polygon == rhs.polygon;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Polygon2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POLYGON2DSTAMPED_H

78
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Pose2D32_
{
typedef Pose2D32_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator> const> ConstPtr;
}; // struct Pose2D32_
typedef ::robot_nav_2d_msgs::Pose2D32_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2D32_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2D32_H

74
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <geometry_msgs/Pose2D.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Pose2DStamped_
{
typedef Pose2DStamped_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Pose2DStamped_
typedef ::robot_nav_2d_msgs::Pose2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.pose == rhs.pose;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Pose2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_POSE2DSTAMPED_H

30
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h Normal file
View File

@ -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 <robot_nav_2d_msgs/SwitchPluginRequest.h>
#include <robot_nav_2d_msgs/SwitchPluginResponse.h>
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

66
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct SwitchPluginRequest_
{
typedef SwitchPluginRequest_<ContainerAllocator> Type;
SwitchPluginRequest_()
: new_plugin() {
}
SwitchPluginRequest_(const ContainerAllocator& _alloc)
: new_plugin(_alloc) {
(void)_alloc;
}
typedef std::basic_string<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _new_plugin_type;
_new_plugin_type new_plugin;
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator> const> ConstPtr;
}; // struct SwitchPluginRequest_
typedef ::robot_nav_2d_msgs::SwitchPluginRequest_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
{
return lhs.new_plugin == rhs.new_plugin;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginRequest_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINREQUEST_H

72
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct SwitchPluginResponse_
{
typedef SwitchPluginResponse_<ContainerAllocator> 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<char, std::char_traits<char>, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<char>> _message_type;
_message_type message;
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator> const> ConstPtr;
}; // struct SwitchPluginResponse_
typedef ::robot_nav_2d_msgs::SwitchPluginResponse_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
{
return lhs.success == rhs.success &&
lhs.message == rhs.message;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::SwitchPluginResponse_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_SWITCHPLUGINRESPONSE_H

78
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2D_
{
typedef Twist2D_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2D_
typedef ::robot_nav_2d_msgs::Twist2D_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D_H

78
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2D32_
{
typedef Twist2D32_<ContainerAllocator> 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_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2D32_
typedef ::robot_nav_2d_msgs::Twist2D32_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
{
return lhs.x == rhs.x &&
lhs.y == rhs.y &&
lhs.theta == rhs.theta;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2D32_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2D32_H

74
src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h Normal file
View File

@ -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 <string>
#include <vector>
#include <memory>
#include <std_msgs/Header.h>
#include <robot_nav_2d_msgs/Twist2D.h>
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct Twist2DStamped_
{
typedef Twist2DStamped_<ContainerAllocator> 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_<ContainerAllocator> _velocity_type;
_velocity_type velocity;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator> const> ConstPtr;
}; // struct Twist2DStamped_
typedef ::robot_nav_2d_msgs::Twist2DStamped_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
{
return lhs.header == rhs.header &&
lhs.velocity == rhs.velocity;
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::Twist2DStamped_<ContainerAllocator2> & rhs)
{
return !(lhs == rhs);
}
} // namespace robot_nav_2d_msgs
#endif // ROBOT_NAV_2D_MSGS_MESSAGE_TWIST2DSTAMPED_H

26
src/Libraries/nav_2d_msgs/include/nav_2d_msgs/UIntBounds.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/UIntBounds.h
View File

@ -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 <string>
@ -11,7 +11,7 @@
#include <memory>
namespace nav_2d_msgs
namespace robot_nav_2d_msgs
{
template <class ContainerAllocator>
struct UIntBounds_
@ -49,22 +49,22 @@ struct UIntBounds_
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::nav_2d_msgs::UIntBounds_<ContainerAllocator> const> ConstPtr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> > Ptr;
typedef std::shared_ptr< ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator> const> ConstPtr;
}; // struct UIntBounds_
typedef ::nav_2d_msgs::UIntBounds_<std::allocator<void> > UIntBounds;
typedef ::robot_nav_2d_msgs::UIntBounds_<std::allocator<void> > 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<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
bool operator==(const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator2> & 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_<ContainerAllocator1> & lhs, con
}
template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::nav_2d_msgs::UIntBounds_<ContainerAllocator2> & rhs)
bool operator!=(const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator1> & lhs, const ::robot_nav_2d_msgs::UIntBounds_<ContainerAllocator2> & 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

Some files were not shown because too many files have changed in this diff Show More