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

update

Browse Source
This commit is contained in:
HiepLM 2025-12-30 09:08:41 +07:00
parent 4962cfbf49
commit 63cb260cb2
112 changed files with 456 additions and 456 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -38,8 +38,8 @@ if (NOT TARGET robot_time)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_time)
endif()
if (NOT TARGET robot_xmlrpcpp)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_xmlrpcpp)
if (NOT TARGET xmlrpcpp)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/xmlrpcpp)
endif()
if (NOT TARGET robot_cpp)

12
doc/architecture_discussion.md
View File

@ -89,10 +89,10 @@ flowchart TB
%% ========== DATA SOURCES ==========
subgraph DataSources["📡 Data Sources"]
direction TB
Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
Goal["<b>🎯 Goal Input</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::PoseStamped<br/>📨 move_base_simple/goal"]
Loc["<b>🌍 Localization</b><br/>━━━━━━━━━━━━━━━━<br/>📍 Pnkx Loc<br/>🗺️ Global Pose<br/>🔄 Pose Updates"]
TF["<b>🔄 Transform System</b><br/>━━━━━━━━━━━━━━━━<br/>📐 tf3::BufferCore<br/>🌐 Coordinate Frames<br/>⏱️ Time Synchronization"]
Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
Odom["<b>🚗 Odometry</b><br/>━━━━━━━━━━━━━━━━<br/>📍 robot_geometry_msgs::Odometry<br/>⚡ Robot Velocity<br/>📊 Position Tracking"]
Laser["<b>📡 Laser Sensors</b><br/>━━━━━━━━━━━━━━━━<br/>🔍 sensor_msgs::LaserScan<br/>🚫 Obstacle Detection<br/>📏 Distance Measurement"]
Map["<b>🗺️ Map Server</b><br/>━━━━━━━━━━━━━━━━<br/>📋 nav_msgs::OccupancyGrid<br/>🏗️ Static Map<br/>📐 Map Metadata"]
style DataSources fill:#FFF9C4,stroke:#F57F17,stroke-width:4px,color:#000
@ -107,7 +107,7 @@ flowchart TB
%% ========== CONTROL LOOP ==========
subgraph ControlLoop["🔄 Control Loop"]
direction LR
CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
CmdVel["<b>⚡ Velocity Command</b><br/>━━━━━━━━━━━━━━━━<br/>📤 robot_geometry_msgs::Twist<br/>📨 cmd_vel<br/>━━━━━━━━━━━━━━━━<br/>➡️ Linear Velocity<br/>🔄 Angular Velocity"]
BaseCtrl["<b>🎮 Base Controller</b><br/>━━━━━━━━━━━━━━━━<br/>🚗 diff_driver_controller<br/>🚲 steer_drive_controller<br/>━━━━━━━━━━━━━━━━<br/>⚙️ Kinematics<br/>🔧 Hardware Interface"]
style ControlLoop fill:#FFEBEE,stroke:#C62828,stroke-width:4px,color:#000
style CmdVel fill:#FFCDD2,stroke:#C62828,stroke-width:3px,font-size:13px
@ -232,9 +232,9 @@ Cần làm rõ:
- Velocity command execution
**Định dạng dữ liệu:**
- `geometry_msgs::Pose2D` / `geometry_msgs::PoseStamped` (vị trí + hướng)
- `geometry_msgs::Twist` (vận tốc linear/angular)
- `std::vector<geometry_msgs::PoseStamped>` (đường đi)
- `robot_geometry_msgs::Pose2D` / `robot_geometry_msgs::PoseStamped` (vị trí + hướng)
- `robot_geometry_msgs::Twist` (vận tốc linear/angular)
- `std::vector<robot_geometry_msgs::PoseStamped>` (đường đi)
- `costmap_2d::Costmap2D` (bản đồ chi phí)
### 3. Thiết kế từng module (interface level)

12
doc/implementation_plan.md
View File

@ -56,8 +56,8 @@
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/odometry_source.h`
```cpp
class IOdometrySource {
virtual geometry_msgs::PoseStamped getCurrentPose() = 0;
virtual geometry_msgs::Twist getCurrentVelocity() = 0;
virtual robot_geometry_msgs::PoseStamped getCurrentPose() = 0;
virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
virtual bool isAvailable() = 0;
};
```
@ -66,7 +66,7 @@ class IOdometrySource {
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/localization_source.h`
```cpp
class ILocalizationSource {
virtual geometry_msgs::PoseStamped getRobotPose() = 0;
virtual robot_geometry_msgs::PoseStamped getRobotPose() = 0;
virtual std::string getGlobalFrame() = 0;
virtual bool isLocalized() = 0;
};
@ -96,9 +96,9 @@ class ISensorSource {
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/base_controller.h`
```cpp
class IBaseController {
virtual bool setVelocity(const geometry_msgs::Twist& cmd_vel) = 0;
virtual bool setVelocity(const robot_geometry_msgs::Twist& cmd_vel) = 0;
virtual bool stop() = 0;
virtual geometry_msgs::Twist getCurrentVelocity() = 0;
virtual robot_geometry_msgs::Twist getCurrentVelocity() = 0;
};
```
@ -120,7 +120,7 @@ class IBaseController {
**File:** `src/Navigations/Cores/move_base_core/include/move_base_core/user_controller.h`
```cpp
class IUserController {
virtual void onGoalReceived(const geometry_msgs::PoseStamped& goal) = 0;
virtual void onGoalReceived(const robot_geometry_msgs::PoseStamped& goal) = 0;
virtual void onNavigationStateChanged(move_base_core::State state) = 0;
virtual void onFeedback(const NavFeedback& feedback) = 0;
};

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

@ -2,10 +2,10 @@
#include <move_base_core/navigation.h>
#include <move_base_core/common.h>
#include <move_base/move_base.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Vector3.h>
#include <tf3/buffer_core.h>
#include <string>
#include <vector>
@ -21,8 +21,8 @@
namespace {
// Convert C PoseStamped to C++ PoseStamped
geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
geometry_msgs::PoseStamped cpp_pose;
robot_geometry_msgs::PoseStamped c_to_cpp_pose_stamped(const PoseStamped* c_pose) {
robot_geometry_msgs::PoseStamped cpp_pose;
cpp_pose.header.seq = c_pose->header.seq;
cpp_pose.header.stamp.sec = c_pose->header.sec;
cpp_pose.header.stamp.nsec = c_pose->header.nsec;
@ -40,7 +40,7 @@ namespace {
}
// Convert C++ PoseStamped to C PoseStamped
void cpp_to_c_pose_stamped(const geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
void cpp_to_c_pose_stamped(const robot_geometry_msgs::PoseStamped& cpp_pose, PoseStamped* c_pose) {
c_pose->header.seq = cpp_pose.header.seq;
c_pose->header.sec = cpp_pose.header.stamp.sec;
c_pose->header.nsec = cpp_pose.header.stamp.nsec;
@ -59,7 +59,7 @@ namespace {
}
// Convert C++ Pose2D to C Pose2D
void cpp_to_c_pose_2d(const geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
void cpp_to_c_pose_2d(const robot_geometry_msgs::Pose2D& cpp_pose, Pose2D* c_pose) {
c_pose->x = cpp_pose.x;
c_pose->y = cpp_pose.y;
c_pose->theta = cpp_pose.theta;
@ -112,12 +112,12 @@ extern "C" bool navigation_offset_goal_2d(double pose_x, double pose_y, double p
return false;
}
geometry_msgs::Pose2D pose;
robot_geometry_msgs::Pose2D pose;
pose.x = pose_x;
pose.y = pose_y;
pose.theta = pose_theta;
geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(pose, std::string(frame_id), offset_distance);
cpp_to_c_pose_stamped(result, out_goal);
return true;
}
@ -128,8 +128,8 @@ extern "C" bool navigation_offset_goal_stamped(const PoseStamped* in_pose, doubl
return false;
}
geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
robot_geometry_msgs::PoseStamped cpp_pose = c_to_cpp_pose_stamped(in_pose);
robot_geometry_msgs::PoseStamped result = robot::move_base_core::offset_goal(cpp_pose, offset_distance);
cpp_to_c_pose_stamped(result, out_goal);
return true;
}
@ -260,10 +260,10 @@ extern "C" bool navigation_set_robot_footprint(NavigationHandle handle, const Po
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
std::vector<geometry_msgs::Point> footprint;
std::vector<robot_geometry_msgs::Point> footprint;
footprint.reserve(point_count);
for (size_t i = 0; i < point_count; ++i) {
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
pt.x = points[i].x;
pt.y = points[i].y;
pt.z = points[i].z;
@ -284,7 +284,7 @@ extern "C" bool navigation_move_to(NavigationHandle handle, const PoseStamped* g
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveTo(cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) {
return false;
@ -300,7 +300,7 @@ extern "C" bool navigation_dock_to(NavigationHandle handle, const char* marker,
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->dockTo(std::string(marker), cpp_goal, xy_goal_tolerance, yaw_goal_tolerance);
} catch (...) {
return false;
@ -315,7 +315,7 @@ extern "C" bool navigation_move_straight_to(NavigationHandle handle, const PoseS
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->moveStraightTo(cpp_goal, xy_goal_tolerance);
} catch (...) {
return false;
@ -330,7 +330,7 @@ extern "C" bool navigation_rotate_to(NavigationHandle handle, const PoseStamped*
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
robot_geometry_msgs::PoseStamped cpp_goal = c_to_cpp_pose_stamped(goal);
return nav->rotateTo(cpp_goal, yaw_goal_tolerance);
} catch (...) {
return false;
@ -378,7 +378,7 @@ extern "C" bool navigation_set_twist_linear(NavigationHandle handle,
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 linear;
linear.x = linear_x;
linear.y = linear_y;
linear.z = linear_z;
@ -396,7 +396,7 @@ extern "C" bool navigation_set_twist_angular(NavigationHandle handle,
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::Vector3 angular;
robot_geometry_msgs::Vector3 angular;
angular.x = angular_x;
angular.y = angular_y;
angular.z = angular_z;
@ -413,7 +413,7 @@ extern "C" bool navigation_get_robot_pose_stamped(NavigationHandle handle, PoseS
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::PoseStamped cpp_pose;
robot_geometry_msgs::PoseStamped cpp_pose;
if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_stamped(cpp_pose, out_pose);
return true;
@ -431,7 +431,7 @@ extern "C" bool navigation_get_robot_pose_2d(NavigationHandle handle, Pose2D* ou
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
geometry_msgs::Pose2D cpp_pose;
robot_geometry_msgs::Pose2D cpp_pose;
if (nav->getRobotPose(cpp_pose)) {
cpp_to_c_pose_2d(cpp_pose, out_pose);
return true;

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

@ -3,8 +3,8 @@
#include <robot/node_handle.h>
#include <robot/console.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>

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

@ -12,7 +12,7 @@
#include <nav_core2/costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <mkt_msgs/Trajectory2D.h>
#include <score_algorithm/trajectory_generator.h>
@ -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<robot_nav_2d_msgs::Pose2DStamped> &plan,
virtual unsigned int getGoalIndex(const robot_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;
/**

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

@ -8,7 +8,7 @@
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <memory>
#include <vector>
#include <geometry_msgs/Vector3.h>
#include <robot_geometry_msgs/Vector3.h>
namespace score_algorithm
{
@ -63,28 +63,28 @@ namespace score_algorithm
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) = 0;
virtual bool setTwistLinear(robot_geometry_msgs::Vector3 linear) = 0;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) = 0;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) = 0;
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) = 0;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) = 0;
/**
* @brief Test to see whether there are more twists to test

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

@ -3,7 +3,7 @@
#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<robot_nav_2d_msgs::Pose2DStamped> &plan,
unsigned int score_algorithm::ScoreAlgorithm::getGoalIndex(const robot_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)
@ -46,7 +46,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
return false;
}
double x_direction_saved = 0.0;
geometry_msgs::PoseArray poseArrayMsg;
robot_geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = robot::Time::now();
poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
@ -80,7 +80,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
{
seq_s.push_back(plan[i].header.seq);
mutes.push_back(plan[i]);
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i]).pose;
robot_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<robot_n
{
seq_s.push_back(plan[i + 1].header.seq);
mutes.push_back(plan[i + 1]);
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan[i + 1]).pose;
robot_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<robot_n
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
else
@ -124,7 +124,7 @@ bool score_algorithm::ScoreAlgorithm::findSubGoalIndex(const std::vector<robot_n
{
seq_s.push_back(plan.back().header.seq);
mutes.push_back(plan.back());
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPoseStamped(plan.back()).pose;
poseArrayMsg.poses.push_back(pose);
}
}
@ -369,7 +369,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
// check if goal already reached
bool goal_already_reached = false;
// geometry_msgs::PoseArray poseArrayMsg;
// robot_geometry_msgs::PoseArray poseArrayMsg;
// poseArrayMsg.header.stamp = robot::Time::now();
// poseArrayMsg.header.frame_id = costmap_robot_->getGlobalFrameID();
// int c = 0;
@ -377,7 +377,7 @@ bool score_algorithm::ScoreAlgorithm::computePlanCommand(const robot_nav_2d_msgs
{
double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal.pose, global_plan.poses[goal_index].pose));
// geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
// robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal.pose);
// poseArrayMsg.poses.push_back(pose);
if (distance < xy_local_goal_tolerance_)

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

@ -4,8 +4,8 @@
#include <score_algorithm/score_algorithm.h>
#include <mkt_algorithm/equation_line.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <kalman/kalman.h>
namespace mkt_algorithm
@ -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 robot_nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
@ -53,30 +53,30 @@ public:
* @param pose
* @param velocity
*/
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
protected:
unsigned int getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const;
double journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const;
double journey(const std::vector<robot_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 robot_nav_2d_msgs::Twist2D& velocity,
const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal,
bool getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal,
double &target_direction);
bool findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
bool findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s);
robot::NodeHandle nh_priv_, nh_;
std::string frame_id_path_;
robot::Publisher reached_intermediate_goals_pub_, current_goal_pub_, closet_robot_goal_pub_;
std::vector<geometry_msgs::Pose2D> reached_intermediate_goals_;
geometry_msgs::Pose2D rev_sub_goal_, sub_goal_;
geometry_msgs::Pose2D goal_;
std::vector<robot_geometry_msgs::Pose2D> reached_intermediate_goals_;
robot_geometry_msgs::Pose2D rev_sub_goal_, sub_goal_;
robot_geometry_msgs::Pose2D goal_;
robot_nav_2d_msgs::Path2D global_plan_;
unsigned int start_index_saved_;

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

@ -1,7 +1,7 @@
#ifndef __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#define __NAV_ALGORITHM_EQUATION_LINE_H_INCLUDED__
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <iostream>
namespace mkt_algorithm
@ -24,8 +24,8 @@ class LineGenerator
{
/* data */
struct LineEquation {
geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
robot_geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
robot_geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
float a, b, c ; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */
};
@ -46,7 +46,7 @@ public:
* @param start_point start point of the line
* @param end_point end point of the line
*/
virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
/**
* @brief Calculating error
@ -54,14 +54,14 @@ public:
* @param[in] pose
* @param[in] rev
*/
virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
/**
* @brief Calculating tolerance
* @param query_pose The pose to check
* @param goal_pose The pose to check against
*/
virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
/* Properties */
LineEquation Leq_;

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

@ -3,8 +3,8 @@
#include <score_algorithm/score_algorithm.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseStamped.h>
namespace mkt_algorithm
{
@ -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 robot_nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_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 robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
protected:
@ -60,7 +60,7 @@ protected:
double steering_fix_wheel_distance_x_;
double steering_fix_wheel_distance_y_;
geometry_msgs::Pose2D goal_;
robot_geometry_msgs::Pose2D goal_;
robot::Time time_last_cmd_;
}; // class GoStraight

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

@ -28,8 +28,8 @@ public:
* @param velocity The robot's current velocity
* @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 robot_nav_2d_msgs::Twist2D& velocity) override;
bool isGoalReached(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose,
const std::vector<robot_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 robot_nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_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 robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
protected:

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

@ -3,8 +3,8 @@
#include <score_algorithm/score_algorithm.h>
#include <pluginlib/class_loader.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseArray.h>
#include <robot_geometry_msgs/PoseStamped.h>
namespace mkt_algorithm
{
@ -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 robot_nav_2d_msgs::Twist2D& velocity,
const geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
virtual bool prepare(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction) override;
/**
@ -51,20 +51,20 @@ public:
* @param pose
* @param velocity
*/
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
virtual robot_nav_2d_msgs::Twist2DStamped calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity) override;
protected:
robot::NodeHandle nh_;
robot::NodeHandle nh_kinematics_;
geometry_msgs::Pose2D sub_goal_;
robot_geometry_msgs::Pose2D sub_goal_;
double steering_fix_wheel_distance_x_;
double steering_fix_wheel_distance_y_;
double angle_threshold_;
double current_direction_;
double velocity_rotate_min_, velocity_rotate_max_;
geometry_msgs::Pose2D goal_;
robot_geometry_msgs::Pose2D goal_;
robot::Time time_last_cmd_;
}; // class RotateToGoal

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

@ -5,8 +5,8 @@
#include <score_algorithm/score_algorithm.h>
#include <boost/dll/alias.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
@ -127,7 +127,7 @@ namespace mkt_algorithm
* @param carrot_pose
* @return carrot message
*/
geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
robot_geometry_msgs::PointStamped createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose);
/**
* @brief Prune global plan
@ -210,7 +210,7 @@ namespace mkt_algorithm
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);
std::vector<robot_geometry_msgs::Point> interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution);
/**
* @brief Cost at pose
@ -291,7 +291,7 @@ namespace mkt_algorithm
// Control frequency
double control_duration_;
std::vector<geometry_msgs::Point> footprint_spec_;
std::vector<robot_geometry_msgs::Point> footprint_spec_;
robot::Time last_actuator_update_;
boost::shared_ptr<KalmanFilter> kf_;

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

@ -22,8 +22,8 @@ void GoStraight::reset()
time_last_cmd_ = robot::Time::now();
}
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,
bool GoStraight::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction)
{
goal_ = goal;
@ -37,12 +37,12 @@ bool GoStraight::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_
return true;
}
robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
robot_nav_2d_msgs::Twist2DStamped GoStraight::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
{
robot_nav_2d_msgs::Twist2DStamped result;
result.header.stamp = robot::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
robot_geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;

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

@ -38,10 +38,10 @@ void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListen
controller_frequency_param_name_ = 1.0;
}
reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("current_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
cmd_raw_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_raw", 1);
reached_intermediate_goals_pub_ = nh_.advertise<robot_geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<robot_geometry_msgs::PoseStamped>("current_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<robot_geometry_msgs::PoseStamped>("closet_robot_goal", 1);
cmd_raw_pub_ = nh_.advertise<robot_geometry_msgs::Twist>("cmd_raw", 1);
x_direction_ = y_direction_= theta_direction_ = 0;
// kalman
@ -87,8 +87,8 @@ void Bicycle::reset()
last_actuator_update_ = robot::Time(0);
}
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,
bool Bicycle::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_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,12 +119,12 @@ bool Bicycle::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2d_msg
return true;
}
robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
{
robot_nav_2d_msgs::Twist2DStamped result;
result.header.stamp = robot::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
robot_geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;
@ -154,7 +154,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
const double d_theta = goal_.theta - sub_goal_.theta;
if(dd < 1e-5)
{
geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
robot_geometry_msgs::Pose2D p = global_plan_.poses[global_plan_.poses.size() - 2];
alpha = velocity.x / (fabs(velocity.x) + 1e-9)*(p.theta - pose.theta);
phi_steer = atan2(2 * L * sin(alpha), 1.0);
}
@ -198,7 +198,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
if(is_filter_)
{
geometry_msgs::Twist msg;
robot_geometry_msgs::Twist msg;
msg.linear.x = result.velocity.x;
msg.angular.z = phi_steer;
cmd_raw_pub_.publish(msg);
@ -220,7 +220,7 @@ robot_nav_2d_msgs::Twist2DStamped Bicycle::calculator(const geometry_msgs::Pose2
return result;
}
unsigned int Bicycle::getGoalIndex(const geometry_msgs::Pose2D &robot_pose, const std::vector<geometry_msgs::Pose2D> &plan,
unsigned int Bicycle::getGoalIndex(const robot_geometry_msgs::Pose2D &robot_pose, const std::vector<robot_geometry_msgs::Pose2D> &plan,
const double distance, unsigned int start_index, unsigned int last_valid_index) const
{
if (start_index >= last_valid_index)
@ -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 robot_nav_2d_msgs::Twist2D& velocity,
const robot_nav_2d_msgs::Path2D &global_plan, geometry_msgs::Pose2D &sub_goal,
bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal,
double &target_direction)
{
const nav_core2::Costmap &costmap = *costmap_;
@ -265,8 +265,8 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
ROS_ERROR_NAMED("PathProgressCritic", "The global plan was empty.");
return false;
}
// 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;
// Chuyển đổi dữ liệu kế hoạch đường đi 'robot_nav_2d_msgs::Path2D' sang 'std::vector<robot_geometry_msgs::Pose2D>'
std::vector<robot_geometry_msgs::Pose2D> plan = global_plan.poses;
std::vector<unsigned int> index_s;
if(!findSubGoalIndex(plan, index_s))
@ -425,7 +425,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
// check if goal already reached
bool goal_already_reached = false;
geometry_msgs::PoseArray poseArrayMsg;
robot_geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = robot::Time::now();
poseArrayMsg.header.frame_id = frame_id_path_;
// std::stringstream ss;
@ -433,7 +433,7 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
{
double distance = fabs(robot_nav_2d_utils::poseDistance(reached_intermediate_goal, plan[goal_index]));
// ss << distance << " ";
geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(reached_intermediate_goal);
poseArrayMsg.poses.push_back(pose);
}
reached_intermediate_goals_pub_.publish(poseArrayMsg);
@ -538,14 +538,14 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
{
if(goal_index < (unsigned int)(plan.size() -1))
{
const geometry_msgs::Pose2D p1 = plan[start_index];
const robot_geometry_msgs::Pose2D p1 = plan[start_index];
int index;
for(index = start_index+1; index < goal_index; index++)
{
geometry_msgs::Pose2D pose = plan[index];
robot_geometry_msgs::Pose2D pose = plan[index];
if(robot_nav_2d_utils::poseDistance(p1, pose) > xy_goal_tolerance_) break;
}
const geometry_msgs::Pose2D p2 = plan[index];
const robot_geometry_msgs::Pose2D p2 = plan[index];
if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
{
const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@ -569,32 +569,32 @@ bool Bicycle::getGoalPose(const geometry_msgs::Pose2D &robot_pose, const robot_n
}
// Publish start_index
geometry_msgs::PoseStamped pose_st = robot_nav_2d_utils::pose2DToPoseStamped(plan[start_index], global_plan.header.frame_id, robot::Time::now());
robot_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 = robot_nav_2d_utils::pose2DToPoseStamped(plan[goal_index], global_plan.header.frame_id, robot::Time::now());
robot_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;
}
bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
bool Bicycle::findSubGoalIndex(const std::vector<robot_geometry_msgs::Pose2D> &plan, std::vector<unsigned int> &index_s)
{
if (plan.empty())
{
return false;
}
double x_direction_saved = 0.0;
geometry_msgs::PoseArray poseArrayMsg;
robot_geometry_msgs::PoseArray poseArrayMsg;
poseArrayMsg.header.stamp = robot::Time::now();
poseArrayMsg.header.frame_id = frame_id_path_;
for (unsigned int i = 1; i < (unsigned int)plan.size(); i++)
{
geometry_msgs::Pose2D p1 = plan[i];
geometry_msgs::Pose2D p2 = plan[i+1];
// geometry_msgs::Pose2D p3 = plan[i+2];
robot_geometry_msgs::Pose2D p1 = plan[i];
robot_geometry_msgs::Pose2D p2 = plan[i+1];
// robot_geometry_msgs::Pose2D p3 = plan[i+2];
double angle1;
if(fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
angle1 = atan2(p2.y - p1.y, p2.x - p1.x);
@ -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 = robot_nav_2d_utils::pose2DToPose(plan[i]);
robot_geometry_msgs::Pose pose = robot_nav_2d_utils::pose2DToPose(plan[i]);
poseArrayMsg.poses.push_back(pose);
}
x_direction_saved = target_direction;
@ -622,7 +622,7 @@ bool Bicycle::findSubGoalIndex(const std::vector<geometry_msgs::Pose2D> &plan, s
return true;
}
double Bicycle::journey(const std::vector<geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
double Bicycle::journey(const std::vector<robot_geometry_msgs::Pose2D> &plan, const unsigned int start_index, const unsigned int last_valid_index) const
{
double S = 0;
if(last_valid_index - start_index > 1)

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

@ -25,8 +25,8 @@ void RotateToGoal::reset()
time_last_cmd_ = robot::Time::now();
}
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,
bool RotateToGoal::prepare( const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D& velocity,
const robot_geometry_msgs::Pose2D& goal, const robot_nav_2d_msgs::Path2D& global_plan,
double& x_direction, double& y_direction, double& theta_direction)
{
goal_ = goal;
@ -38,12 +38,12 @@ bool RotateToGoal::prepare( const geometry_msgs::Pose2D& pose, const robot_nav_2
return true;
}
robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
robot_nav_2d_msgs::Twist2DStamped RotateToGoal::calculator(const robot_geometry_msgs::Pose2D& pose, const robot_nav_2d_msgs::Twist2D velocity)
{
robot_nav_2d_msgs::Twist2DStamped result;
result.header.stamp = robot::Time::now();
geometry_msgs::Pose2D steer_to_baselink_pose;
robot_geometry_msgs::Pose2D steer_to_baselink_pose;
if(!getPose("steer_link", "base_link", steer_to_baselink_pose))
return result;

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

@ -18,15 +18,15 @@ void mkt_algorithm::diff::GoStraight::initialize(
nh_.param("publish_topic", enable_publish_, false);
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
std::vector<geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
double max_length = 0.0;
for (size_t i = 0; i < footprint.size(); ++i)
{
const geometry_msgs::Point &p1 = footprint[i];
const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
const robot_geometry_msgs::Point &p1 = footprint[i];
const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
if (len > max_length)
{
@ -125,7 +125,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
// Dynamically adjust look ahead distance based on the speed
double lookahead_dist = this->getLookAheadDistance(twist);
// Return false if the transformed global plan is empty
geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
robot_geometry_msgs::Pose2D front = transform_plan_.poses.size() > 3 ? (*(transform_plan_.poses.begin() + 1)).pose : transform_plan_.poses.front().pose;
lookahead_dist += fabs(front.y);
// Get lookahead point and publish for visualization

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

@ -20,15 +20,15 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
footprint_spec_ = costmap_robot_->getRobotFootprint();
std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
double max_length = 0.0;
for (size_t i = 0; i < footprint.size(); ++i)
{
const geometry_msgs::Point &p1 = footprint[i];
const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
const robot_geometry_msgs::Point &p1 = footprint[i];
const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
if (len > max_length)
{
@ -218,15 +218,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
return false;
}
std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
std::vector<robot_geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<robot_geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
double max_length = 0.0;
for (size_t i = 0; i < footprint.size(); ++i)
{
const geometry_msgs::Point &p1 = footprint[i];
const geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
const robot_geometry_msgs::Point &p1 = footprint[i];
const robot_geometry_msgs::Point &p2 = footprint[(i + 1) % footprint.size()]; // wrap around to first point
double len = std::hypot(p2.x - p1.x, p2.y - p1.y) / 2.0;
if (len > max_length)
{
@ -308,15 +308,15 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
if ((unsigned int)(compute_plan_.poses.size() > 1) && compute_plan_.poses.back().header.seq != global_plan_.poses.back().header.seq)
{
const geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
const robot_geometry_msgs::Pose2D p2 = compute_plan_.poses.back().pose;
int index;
for (index = (unsigned int)(compute_plan_.poses.size() - 1); index > 0; index--)
{
geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
robot_geometry_msgs::Pose2D pose = compute_plan_.poses[index].pose;
if (robot_nav_2d_utils::poseDistance(p2, pose) > xy_local_goal_tolerance_)
break;
}
const geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
const robot_geometry_msgs::Pose2D p1 = compute_plan_.poses[index].pose;
if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
{
const double path_angle = atan2(p2.y - p1.y, p2.x - p1.x);
@ -344,11 +344,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const robot_nav_2d_msgs:
}
}
geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
robot_geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
? robot_nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
: robot_nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D());
: robot_nav_2d_utils::pose2DToPose(robot_geometry_msgs::Pose2D());
geometry_msgs::Pose back = robot_nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
robot_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);
@ -419,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
nh_priv_.param("allow_rotate", allow_rotate, false);
// double path_distance_to_rotate = hypot(transformed_plan.poses.front().pose.x, transformed_plan.poses.front().pose.y);
geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
robot_geometry_msgs::Pose2D front = transformed_plan.poses.size() > 3 ? transformed_plan.poses[1].pose : transformed_plan.poses.front().pose;
const double distance_allow_rotate = min_journey_squared_;
const double path_distance_to_rotate = hypot(pose.pose.x - compute_plan_.poses.back().pose.x, pose.pose.y - compute_plan_.poses.back().pose.y);
const double journey_plan = compute_plan_.poses.empty() ? distance_allow_rotate : journey(compute_plan_.poses, 0, compute_plan_.poses.size() - 1);
@ -768,9 +768,9 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
return goal_pose_it;
}
geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
robot_geometry_msgs::PointStamped mkt_algorithm::diff::PredictiveTrajectory::createCarrotMsg(const robot_nav_2d_msgs::Pose2DStamped &carrot_pose)
{
geometry_msgs::PointStamped carrot_msg;
robot_geometry_msgs::PointStamped carrot_msg;
carrot_msg.header = carrot_pose.header;
carrot_msg.point.x = carrot_pose.pose.x;
carrot_msg.point.y = carrot_pose.pose.y;
@ -915,8 +915,8 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
// Calculate distance to previous pose
if (i > 0 && max_plan_length > 0)
{
geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
robot_geometry_msgs::Pose2D p1 = global_plan.poses[i].pose;
robot_geometry_msgs::Pose2D p2 = global_plan.poses[i - 1].pose;
plan_length += std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2));
if (fabs(p2.y - p1.y) > 1e-9 || fabs(p2.x - p1.x) > 1e-9)
{
@ -1097,29 +1097,29 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
// ROS_INFO_STREAM_THROTTLE(0.1, ss.str());
}
std::vector<geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::Pose2D pose, std::vector<geometry_msgs::Point> footprint, double resolution)
std::vector<robot_geometry_msgs::Point>
mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(robot_geometry_msgs::Pose2D pose, std::vector<robot_geometry_msgs::Point> footprint, double resolution)
{
// Dịch sang tọa độ tuyệt đối
std::vector<geometry_msgs::Point> abs_footprint;
std::vector<robot_geometry_msgs::Point> abs_footprint;
double cos_th = std::cos(pose.theta);
double sin_th = std::sin(pose.theta);
for (auto pt : footprint)
{
pt.x *= 1.2;
pt.y *= 1.2;
geometry_msgs::Point abs_pt;
robot_geometry_msgs::Point abs_pt;
abs_pt.x = pose.x + pt.x * cos_th - pt.y * sin_th;
abs_pt.y = pose.y + pt.x * sin_th + pt.y * cos_th;
abs_footprint.push_back(abs_pt);
}
std::vector<geometry_msgs::Point> points;
std::vector<robot_geometry_msgs::Point> points;
for (size_t i = 0; i < abs_footprint.size(); ++i)
{
const geometry_msgs::Point &start = abs_footprint[i];
const geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
const robot_geometry_msgs::Point &start = abs_footprint[i];
const robot_geometry_msgs::Point &end = abs_footprint[(i + 1) % abs_footprint.size()];
double dx = end.x - start.x;
double dy = end.y - start.y;
@ -1131,7 +1131,7 @@ mkt_algorithm::diff::PredictiveTrajectory::interpolateFootprint(geometry_msgs::P
if (j == steps && i != abs_footprint.size() - 1)
continue; // tránh lặp
double t = static_cast<double>(j) / steps;
geometry_msgs::Point pt;
robot_geometry_msgs::Point pt;
pt.x = start.x + t * dx;
pt.y = start.y + t * dy;
points.push_back(pt);

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

@ -9,7 +9,7 @@
#include <vector>
#include <memory>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Pose2D.h>
namespace mkt_msgs
{
@ -31,7 +31,7 @@ namespace mkt_msgs
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;
typedef std::vector<robot_geometry_msgs::Pose2D, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot_geometry_msgs::Pose2D>> _poses_type;
_poses_type poses;
typedef std::vector<robot::Duration, typename std::allocator_traits<ContainerAllocator>::template rebind_alloc<robot::Duration>> _time_offsets_type;

12
src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h
View File

@ -1,7 +1,7 @@
#ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
#define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__
#include <robot/node_handle.h>
#include <geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <iostream>
namespace mkt_plugins
@ -24,8 +24,8 @@ class LineGenerator
{
/* data */
struct LineEquation {
geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
robot_geometry_msgs::Pose2D Gi; /* 𝑮𝒊(𝒙𝒊,𝒚𝒊) */
robot_geometry_msgs::Pose2D Gj; /* 𝑮𝒋(𝒙𝒋,𝒚𝒋) */
float a, b, c ; /* 𝒂𝒙+𝒃𝒚+𝒄=𝟎 */
};
@ -46,7 +46,7 @@ public:
* @param start_point start point of the line
* @param end_point end point of the line
*/
virtual bool calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point);
virtual bool calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point);
/**
* @brief Calculating error
@ -54,14 +54,14 @@ public:
* @param[in] pose
* @param[in] rev
*/
virtual void calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev = false);
virtual void calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev = false);
/**
* @brief Calculating tolerance
* @param query_pose The pose to check
* @param goal_pose The pose to check against
*/
virtual double calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose);
virtual double calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose);
/* Properties */
LineEquation Leq_;

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

@ -38,28 +38,28 @@ namespace mkt_plugins
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
bool setTwistLinear(geometry_msgs::Vector3 linear) override;
bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
/**
* @brief Start a new iteration with current velocity
@ -138,7 +138,7 @@ namespace mkt_plugins
* @param dt amount of time in seconds
* @return New pose after dt seconds
*/
virtual geometry_msgs::Pose2D computeNewPosition(const geometry_msgs::Pose2D start_pose,
virtual robot_geometry_msgs::Pose2D computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
const robot_nav_2d_msgs::Twist2D &vel,
const double dt);

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

@ -5,7 +5,7 @@
namespace mkt_plugins
{
bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, geometry_msgs::Pose2D end_point)
bool LineGenerator::calculatorEquationLine(robot_geometry_msgs::Pose2D start_point, robot_geometry_msgs::Pose2D end_point)
{
/**
* Phương trình đường thẳng có dạng:
@ -19,7 +19,7 @@ bool LineGenerator::calculatorEquationLine(geometry_msgs::Pose2D start_point, ge
return Leq_.a != 0 || Leq_.b !=0;
}
void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, bool rev)
void LineGenerator::calculateErrorLine(float Lm, robot_geometry_msgs::Pose2D pose, bool rev)
{
float pha = rev? M_PI : 0;
float Yaw_rad = pose.theta - pha;
@ -87,7 +87,7 @@ void LineGenerator::calculateErrorLine(float Lm, geometry_msgs::Pose2D pose, boo
}
double LineGenerator::calculateTolerance(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose)
double LineGenerator::calculateTolerance(const robot_geometry_msgs::Pose2D& query_pose, const robot_geometry_msgs::Pose2D& goal_pose)
{
/**
* Phương trình đường thẳng có dạng:

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

@ -59,7 +59,7 @@ namespace mkt_plugins
kinematics_->setDirect(direct);
}
bool StandardTrajectoryGenerator::setTwistLinear(geometry_msgs::Vector3 linear)
bool StandardTrajectoryGenerator::setTwistLinear(robot_geometry_msgs::Vector3 linear)
{
try
{
@ -93,9 +93,9 @@ namespace mkt_plugins
return true;
}
geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistLinear(bool direct)
{
geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 linear;
try
{
if (direct)
@ -116,7 +116,7 @@ namespace mkt_plugins
return linear;
}
bool StandardTrajectoryGenerator::setTwistAngular(geometry_msgs::Vector3 angular)
bool StandardTrajectoryGenerator::setTwistAngular(robot_geometry_msgs::Vector3 angular)
{
try
{
@ -130,9 +130,9 @@ namespace mkt_plugins
return true;
}
geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
robot_geometry_msgs::Vector3 StandardTrajectoryGenerator::getTwistAngular(bool direct)
{
geometry_msgs::Vector3 angular;
robot_geometry_msgs::Vector3 angular;
try
{
if (direct)
@ -205,7 +205,7 @@ namespace mkt_plugins
traj.velocity = cmd_vel;
// simulate the trajectory
geometry_msgs::Pose2D pose = start_pose.pose;
robot_geometry_msgs::Pose2D pose = start_pose.pose;
robot_nav_2d_msgs::Twist2D vel = start_vel;
double running_time = 0.0;
std::vector<double> steps = getTimeSteps(cmd_vel);
@ -245,10 +245,10 @@ namespace mkt_plugins
return new_vel;
}
geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const geometry_msgs::Pose2D start_pose,
robot_geometry_msgs::Pose2D StandardTrajectoryGenerator::computeNewPosition(const robot_geometry_msgs::Pose2D start_pose,
const robot_nav_2d_msgs::Twist2D &vel, const double dt)
{
geometry_msgs::Pose2D new_pose;
robot_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;
new_pose.y = start_pose.y + (vel.x * sin(start_pose.theta) + vel.y * sin(M_PI_2 + start_pose.theta)) * dt;
new_pose.theta = start_pose.theta + vel.theta * dt;

2
src/Algorithms/Packages/global_planners/custom_planner

@ -1 +1 @@
Subproject commit 377ebe3d6f66c6a1c7f2d1a1d76fd1b330a93952
Subproject commit 6d55c6c4beb86ae77c20cba3f26c6231a16912fa

10
src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h
View File

@ -4,7 +4,7 @@
#include <iostream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <nav_core/base_global_planner.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <robot/node_handle.h>
@ -48,9 +48,9 @@ public:
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan);
virtual bool makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan);
/**
* @brief Create a new TwoPointsPlanner instance
@ -72,7 +72,7 @@ protected:
std::string name_;
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
std::vector<geometry_msgs::Point> footprint_;
std::vector<robot_geometry_msgs::Point> footprint_;
unsigned int current_env_width_;
unsigned int current_env_height_;

10
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@ -100,9 +100,9 @@ namespace two_points_planner
}
}
bool TwoPointsPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal,
std::vector<geometry_msgs::PoseStamped>& plan)
bool TwoPointsPlanner::makePlan(const robot_geometry_msgs::PoseStamped& start,
const robot_geometry_msgs::PoseStamped& goal,
std::vector<robot_geometry_msgs::PoseStamped>& plan)
{
if (!initialized_)
{
@ -188,7 +188,7 @@ namespace two_points_planner
{
double fraction = static_cast<double>(i) / num_points;
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header.stamp = plan_time;
pose.header.frame_id = costmap_robot_->getGlobalFrameID();
pose.pose.position.x = start.pose.position.x + fraction * dx;
@ -199,7 +199,7 @@ namespace two_points_planner
tf3::Quaternion interpolated_quat;
interpolated_quat.setRPY(0, 0, theta);
// Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
// Chuyển đổi trực tiếp từ tf3::Quaternion sang robot_geometry_msgs::Quaternion
pose.pose.orientation.x = interpolated_quat.x();
pose.pose.orientation.y = interpolated_quat.y();
pose.pose.orientation.z = interpolated_quat.z();

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

@ -140,8 +140,8 @@ namespace pnkx_local_planner
nav_core::BaseGlobalPlanner::Ptr docking_planner_;
score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
geometry_msgs::Vector3 linear_;
geometry_msgs::Vector3 angular_;
robot_geometry_msgs::Vector3 linear_;
robot_geometry_msgs::Vector3 angular_;
private:
/**

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

@ -67,28 +67,28 @@ namespace pnkx_local_planner
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
bool setTwistLinear(geometry_msgs::Vector3 linear) override;
bool setTwistLinear(robot_geometry_msgs::Vector3 linear) override;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistLinear(bool direct) override;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
virtual bool setTwistAngular(robot_geometry_msgs::Vector3 angular) override;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
virtual robot_geometry_msgs::Vector3 getTwistAngular(bool direct) override;
/**
* @brief Check if the kinematic parameters are currently locked

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

@ -23,7 +23,7 @@ namespace pnkx_local_planner
bool transformGlobalPose(TFListenerPtr tf, const std::string &global_frame,
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);
double getSquareDistance(const robot_geometry_msgs::Pose2D &pose_a, const robot_geometry_msgs::Pose2D &pose_b);
/**
* @brief Transforms the global plan of the robot from the planner frame to the local frame (modified).

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

@ -298,7 +298,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
if (!dkpl_.empty() && dkpl_.front()->initialized_ && dkpl_.front()->is_detected_ && !dkpl_.front()->is_goal_reached_)
{
this->lock();
geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
robot_geometry_msgs::Vector3 linear = dkpl_.front()->linear_;
traj_generator_->setTwistLinear(linear);
linear.x *= (-1);
traj_generator_->setTwistLinear(linear);
@ -733,9 +733,9 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(ro
bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
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 = 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;
robot_geometry_msgs::PoseStamped start = robot_nav_2d_utils::pose2DToPoseStamped(local_pose);
robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(local_goal);
std::vector<robot_geometry_msgs::PoseStamped> docking_plan;
if (!docking_planner_->makePlan(start, goal, docking_plan))
{

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

@ -299,7 +299,7 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::ScoreAlg
return cmd_vel;
}
bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3 linear)
bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(robot_geometry_msgs::Vector3 linear)
{
try
{
@ -330,7 +330,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(geometry_msgs::Vector3
}
}
geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool direct)
{
try
{
@ -345,7 +345,7 @@ geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinear(bool
}
}
bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector3 angular)
bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(robot_geometry_msgs::Vector3 angular)
{
try
{
@ -361,7 +361,7 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(geometry_msgs::Vector
}
}
geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngular(bool direct)
{
try
{

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

@ -10,9 +10,9 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st
if (!tf)
return false;
geometry_msgs::PoseStamped global_pose_stamped;
robot_geometry_msgs::PoseStamped global_pose_stamped;
tf3::toMsg(tf3::Transform::getIdentity(), global_pose_stamped.pose);
geometry_msgs::PoseStamped robot_pose;
robot_geometry_msgs::PoseStamped robot_pose;
tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = base_frame_id;
robot_pose.header.stamp = robot::Time();
@ -80,7 +80,7 @@ bool pnkx_local_planner::transformGlobalPose(TFListenerPtr tf, const std::string
return result;
}
double pnkx_local_planner::getSquareDistance(const geometry_msgs::Pose2D& pose_a, const geometry_msgs::Pose2D& pose_b)
double pnkx_local_planner::getSquareDistance(const robot_geometry_msgs::Pose2D& pose_a, const robot_geometry_msgs::Pose2D& pose_b)
{
double x_diff = pose_a.x - pose_b.x;
double y_diff = pose_a.y - pose_b.y;
@ -148,15 +148,15 @@ bool pnkx_local_planner::transformGlobalPlan(
double sq_prune_dist = 0.1;
std::vector<geometry_msgs::Point> footprint = costmap->getRobotFootprint();
std::vector<robot_geometry_msgs::Point> footprint = costmap->getRobotFootprint();
size_t n = footprint.size();
if (n > 1)
{
double max_length = 0.0;
for (size_t i = 0; i < n; ++i)
{
const geometry_msgs::Point& p1 = footprint[i];
const geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
const robot_geometry_msgs::Point& p1 = footprint[i];
const robot_geometry_msgs::Point& p2 = footprint[(i + 1) % n]; // wrap around to first point
double len = std::hypot(p2.x - p1.x, p2.y - p1.y);
if (len > max_length)
{

2
src/Libraries/costmap_2d

@ -1 +1 @@
Subproject commit 71adf1390fe7c1bad3823d25a06db43a9c46278b
Subproject commit 2c3d7d586d1a664573b99f45d8521950d46db30a

2
src/Libraries/data_convert

@ -1 +1 @@
Subproject commit 8b22de38ac16eccce348e505e12cfafdf33e2943
Subproject commit b1aaa1a9463853ec16574b27182c5753c630a407

2
src/Libraries/geometry2

@ -1 +1 @@
Subproject commit 3c51b228ec4d3f3223225142d3c13e485e01de52
Subproject commit 2987c1a481390e4d0bf08cf97aee3bc758d23ad1

2
src/Libraries/laser_geometry

@ -1 +1 @@
Subproject commit 1b06dd9122b46f8d8a7c0414d8fa5043656f7933
Subproject commit a183d4bb7bdd1e6eb44be08911cc1a4e4590c4e4

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

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/ComplexPolygon2D.h
View File

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

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfChars.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfCharsUpdate.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoubles.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/NavGridOfDoublesUpdate.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Path2D.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Point2D.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2D.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DCollection.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Polygon2DStamped.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2D32.h
View File

4
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Pose2DStamped.h
View File

@ -11,7 +11,7 @@
#include <memory>
#include <std_msgs/Header.h>
#include <geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Pose2D.h>
namespace robot_nav_2d_msgs
{
@ -35,7 +35,7 @@ struct Pose2DStamped_
typedef ::std_msgs::Header _header_type;
_header_type header;
typedef ::geometry_msgs::Pose2D _pose_type;
typedef ::robot_geometry_msgs::Pose2D _pose_type;
_pose_type pose;

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPlugin.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginRequest.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/SwitchPluginResponse.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2D32.h
View File

0
src/Libraries/nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h → src/Libraries/robot_nav_2d_msgs/include/robot_nav_2d_msgs/Twist2DStamped.h
View File

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

0
src/Libraries/nav_2d_msgs/package.xml → src/Libraries/robot_nav_2d_msgs/package.xml
View File

14
src/Libraries/nav_2d_utils/CMakeLists.txt → src/Libraries/robot_nav_2d_utils/CMakeLists.txt
View File

@ -28,7 +28,7 @@ target_include_directories(conversions
target_link_libraries(conversions
PUBLIC
robot_nav_2d_msgs
geometry_msgs
robot_geometry_msgs
nav_msgs
nav_grid
nav_core2
@ -52,7 +52,7 @@ target_include_directories(path_ops
target_link_libraries(path_ops
PUBLIC
robot_nav_2d_msgs
geometry_msgs
robot_geometry_msgs
robot_cpp
)
@ -66,17 +66,17 @@ target_include_directories(polygons
if(robot_xmlrpcpp_FOUND)
target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(polygons
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${robot_xmlrpcpp_LIBRARIES})
elseif(XmlRpcCpp_FOUND)
target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
target_link_libraries(polygons
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${XmlRpcCpp_LIBRARIES})
else()
target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(polygons
PUBLIC robot_nav_2d_msgs geometry_msgs robot_cpp
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${robot_xmlrpcpp_LIBRARIES})
endif()
@ -110,7 +110,7 @@ target_include_directories(tf_help
target_link_libraries(tf_help
PUBLIC
robot_nav_2d_msgs
geometry_msgs
robot_geometry_msgs
nav_core2
tf3
robot_cpp
@ -166,5 +166,5 @@ message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help")
message(STATUS "Dependencies: robot_nav_2d_msgs, geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
message(STATUS "=================================")

0
src/Libraries/nav_2d_utils/README.md → src/Libraries/robot_nav_2d_utils/README.md
View File

22
src/Libraries/nav_2d_utils/doc/Conversions.md → src/Libraries/robot_nav_2d_utils/doc/Conversions.md
View File

@ -5,35 +5,35 @@
## Twist Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
| `Twist2D twist3Dto2D(geometry_msgs::Twist)` | `geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
| `Twist2D twist3Dto2D(robot_geometry_msgs::Twist)` | `robot_geometry_msgs::Twist twist2Dto3D(Twist2D cmd_vel_2d)`
## Point Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
| `Point2D pointToPoint2D(geometry_msgs::Point)` | `geometry_msgs::Point pointToPoint3D(Point2D)`
| `Point2D pointToPoint2D(geometry_msgs::Point32)` | `geometry_msgs::Point32 pointToPoint32(Point2D)`
| `Point2D pointToPoint2D(robot_geometry_msgs::Point)` | `robot_geometry_msgs::Point pointToPoint3D(Point2D)`
| `Point2D pointToPoint2D(robot_geometry_msgs::Point32)` | `robot_geometry_msgs::Point32 pointToPoint32(Point2D)`
## Pose Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
| `Pose2DStamped poseStampedToPose2D(robot_geometry_msgs::PoseStamped)` | `robot_geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
||`robot_geometry_msgs::PoseStamped pose2DToPoseStamped(robot_geometry_msgs::Pose2D, std::string, robot::Time)`|
||`robot_geometry_msgs::Pose pose2DToPose(robot_geometry_msgs::Pose2D)`|
| `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
## Path Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
| `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
| `Path2D posesToPath2D(std::vector<robot_geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<robot_geometry_msgs::Pose2D>, std::string, robot::Time)`
Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
Also, `nav_msgs::Path posesToPath(std::vector<robot_geometry_msgs::PoseStamped>)`
## Polygon Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
| `Polygon2D polygon3Dto2D(geometry_msgs::Polygon)` |`geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
| `Polygon2DStamped polygon3Dto2D(geometry_msgs::PolygonStamped)` | `geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
| `Polygon2D polygon3Dto2D(robot_geometry_msgs::Polygon)` |`robot_geometry_msgs::Polygon polygon2Dto3D(Polygon2D)`
| `Polygon2DStamped polygon3Dto2D(robot_geometry_msgs::PolygonStamped)` | `robot_geometry_msgs::PolygonStamped polygon2Dto3D(Polygon2DStamped)`
## Info Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
@ -46,7 +46,7 @@ Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`
| to two dimensional pose | to three dimensional pose |
| -- | -- |
| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
| `Pose2D getOrigin2D(nav_grid::NavGridInfo)` | `robot_geometry_msgs::Pose getOrigin3D(nav_grid::NavGridInfo)`|
## Bounds Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |

0
src/Libraries/nav_2d_utils/doc/PluginMux.md → src/Libraries/robot_nav_2d_utils/doc/PluginMux.md
View File

0
src/Libraries/nav_2d_utils/doc/PolygonsAndFootprints.md → src/Libraries/robot_nav_2d_utils/doc/PolygonsAndFootprints.md
View File

0
src/Libraries/nav_2d_utils/include/mapbox/LICENSE → src/Libraries/robot_nav_2d_utils/include/mapbox/LICENSE
View File

0
src/Libraries/nav_2d_utils/include/mapbox/NOTES → src/Libraries/robot_nav_2d_utils/include/mapbox/NOTES
View File

0
src/Libraries/nav_2d_utils/include/mapbox/earcut.hpp → src/Libraries/robot_nav_2d_utils/include/mapbox/earcut.hpp
View File

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/bounds.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
View File

46
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/conversions.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
View File

@ -35,10 +35,10 @@
#ifndef ROBOT_NAV_2D_UTILS_CONVERSIONS_H
#define ROBOT_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 <robot_geometry_msgs/Point.h>
#include <robot_geometry_msgs/Pose.h>
#include <robot_geometry_msgs/Twist.h>
#include <robot_geometry_msgs/PolygonStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Point2D.h>
@ -58,42 +58,42 @@
namespace robot_nav_2d_utils
{
// Twist Transformations
::geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::geometry_msgs::Twist &cmd_vel);
::robot_geometry_msgs::Twist twist2Dto3D(const ::robot_nav_2d_msgs::Twist2D &cmd_vel_2d);
::robot_nav_2d_msgs::Twist2D twist3Dto2D(const ::robot_geometry_msgs::Twist &cmd_vel);
// Point Transformations
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point &point);
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::geometry_msgs::Point32 &point);
::geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
::geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point &point);
::robot_nav_2d_msgs::Point2D pointToPoint2D(const ::robot_geometry_msgs::Point32 &point);
::robot_geometry_msgs::Point pointToPoint3D(const ::robot_nav_2d_msgs::Point2D &point);
::robot_geometry_msgs::Point32 pointToPoint32(const ::robot_nav_2d_msgs::Point2D &point);
// Pose Transformations
// ::robot_nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped<tf::Pose>& pose);
::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::geometry_msgs::PoseStamped &pose);
::geometry_msgs::Pose pose2DToPose(const ::geometry_msgs::Pose2D &pose2d);
::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
::geometry_msgs::PoseStamped pose2DToPoseStamped(const ::geometry_msgs::Pose2D &pose2d,
::robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const ::robot_geometry_msgs::PoseStamped &pose);
::robot_geometry_msgs::Pose pose2DToPose(const ::robot_geometry_msgs::Pose2D &pose2d);
::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_nav_2d_msgs::Pose2DStamped &pose2d);
::robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const ::robot_geometry_msgs::Pose2D &pose2d,
const ::std::string &frame, const robot::Time &stamp);
// Path Transformations
::robot_nav_2d_msgs::Path2D pathToPath(const nav_msgs::Path &path);
nav_msgs::Path posesToPath(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::geometry_msgs::PoseStamped> &poses);
nav_msgs::Path poses2DToPath(const ::std::vector<::geometry_msgs::Pose2D> &poses,
nav_msgs::Path posesToPath(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
::robot_nav_2d_msgs::Path2D posesToPath2D(const ::std::vector<::robot_geometry_msgs::PoseStamped> &poses);
nav_msgs::Path poses2DToPath(const ::std::vector<::robot_geometry_msgs::Pose2D> &poses,
const ::std::string &frame, const robot::Time &stamp);
nav_msgs::Path pathToPath(const ::robot_nav_2d_msgs::Path2D &path2d);
// Polygon Transformations
::geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::geometry_msgs::Polygon &polygon_3d);
::geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::geometry_msgs::PolygonStamped &polygon_3d);
::robot_geometry_msgs::Polygon polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2D &polygon_2d);
::robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const ::robot_geometry_msgs::Polygon &polygon_3d);
::robot_geometry_msgs::PolygonStamped polygon2Dto3D(const ::robot_nav_2d_msgs::Polygon2DStamped &polygon_2d);
::robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const ::robot_geometry_msgs::PolygonStamped &polygon_3d);
// Info Transformations
::robot_nav_2d_msgs::NavGridInfo toMsg(const nav_grid::NavGridInfo &info);
nav_grid::NavGridInfo fromMsg(const ::robot_nav_2d_msgs::NavGridInfo &msg);
::geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
::geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info);
::robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info);
::robot_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);

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/footprint.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/footprint.h
View File

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/geometry_help.h
View File

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/odom_subscriber.h
View File

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/parameters.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/parameters.h
View File

4
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/path_ops.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/path_ops.h
View File

@ -42,7 +42,7 @@ namespace robot_nav_2d_utils
/**
* @brief Calculate the linear distance between two poses
*/
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1);
double poseDistance(const robot_geometry_msgs::Pose2D &pose0, const robot_geometry_msgs::Pose2D &pose1);
/**
* @brief Calculate the length of the plan, starting from the given index
@ -52,7 +52,7 @@ namespace robot_nav_2d_utils
/**
* @brief Calculate the length of the plan from the pose on the plan closest to the given pose
*/
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose);
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const robot_geometry_msgs::Pose2D &query_pose);
/**
* @brief Increase plan resolution to match that of the costmap by adding points linearly between points

0
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/plugin_mux.h
View File

4
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/polygons.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/polygons.h
View File

@ -37,7 +37,7 @@
#include <robot_nav_2d_msgs/Polygon2D.h>
#include <robot_nav_2d_msgs/ComplexPolygon2D.h>
#include <geometry_msgs/Pose2D.h>
#include <robot_geometry_msgs/Pose2D.h>
#include <robot_xmlrpcpp/XmlRpcValue.h>
#include <vector>
#include <string>
@ -148,7 +148,7 @@ bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msg
* @return A new moved polygon
*/
robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
const geometry_msgs::Pose2D& pose);
const robot_geometry_msgs::Pose2D& pose);
/**
* @brief Check if a given point is inside of a polygon

6
src/Libraries/nav_2d_utils/include/robot_nav_2d_utils/tf_help.h → src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
View File

@ -36,7 +36,7 @@
#define ROBOT_NAV_2D_UTILS_TF_HELP_H
#include <nav_core2/common.h>
#include <geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <string>
@ -54,7 +54,7 @@ namespace robot_nav_2d_utils
* @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 ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
const bool extrapolation_fallback = true);
/**
@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
* @param frame_id Frame to transform the pose into
* @return The resulting transformed pose
*/
geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
robot_geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
const ::std::string &frame_id);
} // namespace robot_nav_2d_utils

0
src/Libraries/nav_2d_utils/package.xml → src/Libraries/robot_nav_2d_utils/package.xml
View File

0
src/Libraries/nav_2d_utils/setup.py → src/Libraries/robot_nav_2d_utils/setup.py
View File

0
src/Libraries/nav_2d_utils/src/bounds.cpp → src/Libraries/robot_nav_2d_utils/src/bounds.cpp
View File

58
src/Libraries/nav_2d_utils/src/conversions.cpp → src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@ -38,16 +38,16 @@
namespace robot_nav_2d_utils
{
geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d)
robot_geometry_msgs::Twist twist2Dto3D(const robot_nav_2d_msgs::Twist2D &cmd_vel_2d)
{
geometry_msgs::Twist cmd_vel;
robot_geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = cmd_vel_2d.x;
cmd_vel.linear.y = cmd_vel_2d.y;
cmd_vel.angular.z = cmd_vel_2d.theta;
return cmd_vel;
}
robot_nav_2d_msgs::Twist2D twist3Dto2D(const geometry_msgs::Twist &cmd_vel)
robot_nav_2d_msgs::Twist2D twist3Dto2D(const robot_geometry_msgs::Twist &cmd_vel)
{
robot_nav_2d_msgs::Twist2D cmd_vel_2d;
cmd_vel_2d.x = cmd_vel.linear.x;
@ -56,7 +56,7 @@ namespace robot_nav_2d_utils
return cmd_vel_2d;
}
robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point &point)
robot_nav_2d_msgs::Point2D pointToPoint2D(const robot_geometry_msgs::Point &point)
{
robot_nav_2d_msgs::Point2D output;
output.x = point.x;
@ -64,7 +64,7 @@ namespace robot_nav_2d_utils
return output;
}
robot_nav_2d_msgs::Point2D pointToPoint2D(const geometry_msgs::Point32 &point)
robot_nav_2d_msgs::Point2D pointToPoint2D(const robot_geometry_msgs::Point32 &point)
{
robot_nav_2d_msgs::Point2D output;
output.x = point.x;
@ -72,17 +72,17 @@ namespace robot_nav_2d_utils
return output;
}
geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point)
robot_geometry_msgs::Point pointToPoint3D(const robot_nav_2d_msgs::Point2D &point)
{
geometry_msgs::Point output;
robot_geometry_msgs::Point output;
output.x = point.x;
output.y = point.y;
return output;
}
geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point)
robot_geometry_msgs::Point32 pointToPoint32(const robot_nav_2d_msgs::Point2D &point)
{
geometry_msgs::Point32 output;
robot_geometry_msgs::Point32 output;
output.x = point.x;
output.y = point.y;
return output;
@ -99,7 +99,7 @@ namespace robot_nav_2d_utils
// return pose2d;
// }
robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const geometry_msgs::PoseStamped &pose)
robot_nav_2d_msgs::Pose2DStamped poseStampedToPose2D(const robot_geometry_msgs::PoseStamped &pose)
{
robot_nav_2d_msgs::Pose2DStamped pose2d;
pose2d.header = pose.header;
@ -109,27 +109,27 @@ namespace robot_nav_2d_utils
return pose2d;
}
geometry_msgs::Pose pose2DToPose(const geometry_msgs::Pose2D &pose2d)
robot_geometry_msgs::Pose pose2DToPose(const robot_geometry_msgs::Pose2D &pose2d)
{
geometry_msgs::Pose pose;
robot_geometry_msgs::Pose pose;
pose.position.x = pose2d.x;
pose.position.y = pose2d.y;
// pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
return pose;
}
geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d)
robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_nav_2d_msgs::Pose2DStamped &pose2d)
{
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
pose.header = pose2d.header;
pose.pose = pose2DToPose(pose2d.pose);
return pose;
}
// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
// robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
// const std::string& frame, const robot::Time& stamp)
// {
// geometry_msgs::PoseStamped pose;
// robot_geometry_msgs::PoseStamped pose;
// pose.header.frame_id = frame;
// pose.header.stamp = stamp;
// pose.pose.position.x = pose2d.x;
@ -152,7 +152,7 @@ namespace robot_nav_2d_utils
return path2d;
}
nav_msgs::Path posesToPath(const std::vector<geometry_msgs::PoseStamped> &poses)
nav_msgs::Path posesToPath(const std::vector<robot_geometry_msgs::PoseStamped> &poses)
{
nav_msgs::Path path;
if (poses.empty())
@ -168,7 +168,7 @@ namespace robot_nav_2d_utils
return path;
}
robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped> &poses)
robot_nav_2d_msgs::Path2D posesToPath2D(const std::vector<robot_geometry_msgs::PoseStamped> &poses)
{
robot_nav_2d_msgs::Path2D path;
if (poses.empty())
@ -186,7 +186,7 @@ namespace robot_nav_2d_utils
return path;
}
// nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
// nav_msgs::Path poses2DToPath(const std::vector<robot_geometry_msgs::Pose2D>& poses,
// const std::string& frame, const robot::Time& stamp)
// {
// nav_msgs::Path path;
@ -213,9 +213,9 @@ namespace robot_nav_2d_utils
return path;
}
geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d)
robot_geometry_msgs::Polygon polygon2Dto3D(const robot_nav_2d_msgs::Polygon2D &polygon_2d)
{
geometry_msgs::Polygon polygon;
robot_geometry_msgs::Polygon polygon;
polygon.points.reserve(polygon_2d.points.size());
for (const auto &pt : polygon_2d.points)
{
@ -224,7 +224,7 @@ namespace robot_nav_2d_utils
return polygon;
}
robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const geometry_msgs::Polygon &polygon_3d)
robot_nav_2d_msgs::Polygon2D polygon3Dto2D(const robot_geometry_msgs::Polygon &polygon_3d)
{
robot_nav_2d_msgs::Polygon2D polygon;
polygon.points.reserve(polygon_3d.points.size());
@ -235,15 +235,15 @@ namespace robot_nav_2d_utils
return polygon;
}
geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d)
robot_geometry_msgs::PolygonStamped polygon2Dto3D(const robot_nav_2d_msgs::Polygon2DStamped &polygon_2d)
{
geometry_msgs::PolygonStamped polygon;
robot_geometry_msgs::PolygonStamped polygon;
polygon.header = polygon_2d.header;
polygon.polygon = polygon2Dto3D(polygon_2d.polygon);
return polygon;
}
robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const geometry_msgs::PolygonStamped &polygon_3d)
robot_nav_2d_msgs::Polygon2DStamped polygon3Dto2D(const robot_geometry_msgs::PolygonStamped &polygon_3d)
{
robot_nav_2d_msgs::Polygon2DStamped polygon;
polygon.header = polygon_3d.header;
@ -291,18 +291,18 @@ namespace robot_nav_2d_utils
return info;
}
geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
robot_geometry_msgs::Pose getOrigin3D(const nav_grid::NavGridInfo &info)
{
geometry_msgs::Pose origin;
robot_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)
robot_geometry_msgs::Pose2D getOrigin2D(const nav_grid::NavGridInfo &info)
{
geometry_msgs::Pose2D origin;
robot_geometry_msgs::Pose2D origin;
origin.x = info.origin_x;
origin.y = info.origin_y;
return origin;

0
src/Libraries/nav_2d_utils/src/footprint.cpp → src/Libraries/robot_nav_2d_utils/src/footprint.cpp
View File

8
src/Libraries/nav_2d_utils/src/path_ops.cpp → src/Libraries/robot_nav_2d_utils/src/path_ops.cpp
View File

@ -40,7 +40,7 @@
namespace robot_nav_2d_utils
{
double poseDistance(const geometry_msgs::Pose2D &pose0, const geometry_msgs::Pose2D &pose1)
double poseDistance(const robot_geometry_msgs::Pose2D &pose0, const robot_geometry_msgs::Pose2D &pose1)
{
return hypot(pose0.x - pose1.x, pose0.y - pose1.y);
}
@ -55,7 +55,7 @@ namespace robot_nav_2d_utils
return length;
}
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const geometry_msgs::Pose2D &query_pose)
double getPlanLength(const robot_nav_2d_msgs::Path2D &plan, const robot_geometry_msgs::Pose2D &query_pose)
{
if (plan.poses.size() == 0)
return 0.0;
@ -87,10 +87,10 @@ namespace robot_nav_2d_utils
global_plan_out.poses.push_back(last_stp);
double sq_resolution = resolution * resolution;
geometry_msgs::Pose2D last = last_stp.pose;
robot_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;
robot_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)
{

2
src/Libraries/nav_2d_utils/src/polygons.cpp → src/Libraries/robot_nav_2d_utils/src/polygons.cpp
View File

@ -365,7 +365,7 @@ bool equals(const robot_nav_2d_msgs::Polygon2D& polygon0, const robot_nav_2d_msg
}
robot_nav_2d_msgs::Polygon2D movePolygonToPose(const robot_nav_2d_msgs::Polygon2D& polygon,
const geometry_msgs::Pose2D& pose)
const robot_geometry_msgs::Pose2D& pose)
{
robot_nav_2d_msgs::Polygon2D new_polygon;
new_polygon.points.resize(polygon.points.size());

10
src/Libraries/nav_2d_utils/src/tf_help.cpp → src/Libraries/robot_nav_2d_utils/src/tf_help.cpp
View File

@ -42,7 +42,7 @@
namespace robot_nav_2d_utils
{
bool transformPose(const TFListenerPtr tf, const ::std::string frame,
const ::geometry_msgs::PoseStamped &in_pose, ::geometry_msgs::PoseStamped &out_pose,
const ::robot_geometry_msgs::PoseStamped &in_pose, ::robot_geometry_msgs::PoseStamped &out_pose,
const bool extrapolation_fallback)
{
// if (in_pose.header.frame_id == frame)
@ -60,7 +60,7 @@ namespace robot_nav_2d_utils
// {
// if (!extrapolation_fallback)
// throw;
// geometry_msgs::PoseStamped latest_in_pose;
// robot_geometry_msgs::PoseStamped latest_in_pose;
// latest_in_pose.header.frame_id = in_pose.header.frame_id;
// latest_in_pose.pose = in_pose.pose;
// tf->transform(latest_in_pose, out_pose, frame);
@ -78,8 +78,8 @@ namespace robot_nav_2d_utils
const ::robot_nav_2d_msgs::Pose2DStamped &in_pose, ::robot_nav_2d_msgs::Pose2DStamped &out_pose,
const bool extrapolation_fallback)
{
geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
geometry_msgs::PoseStamped out_3d_pose;
robot_geometry_msgs::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
robot_geometry_msgs::PoseStamped out_3d_pose;
bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, extrapolation_fallback);
if (ret)
@ -89,7 +89,7 @@ namespace robot_nav_2d_utils
return ret;
}
::geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
::robot_geometry_msgs::Pose2D transformStampedPose(const TFListenerPtr tf, const ::robot_nav_2d_msgs::Pose2DStamped &pose,
const ::std::string &frame_id)
{
robot_nav_2d_msgs::Pose2DStamped local_pose;

0
src/Libraries/nav_2d_utils/test/bounds_test.cpp → src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp
View File

0
src/Libraries/nav_2d_utils/test/compress_test.cpp → src/Libraries/robot_nav_2d_utils/test/compress_test.cpp
View File

0
src/Libraries/nav_2d_utils/test/param_tests.cpp → src/Libraries/robot_nav_2d_utils/test/param_tests.cpp
View File

0
src/Libraries/nav_2d_utils/test/param_tests.launch → src/Libraries/robot_nav_2d_utils/test/param_tests.launch
View File

2
src/Libraries/nav_2d_utils/test/polygon_tests.cpp → src/Libraries/robot_nav_2d_utils/test/polygon_tests.cpp
View File

@ -148,7 +148,7 @@ TEST(Polygon2D, broken_arrays)
TEST(Polygon2D, test_move)
{
Polygon2D square = polygonFromString("[[0.5, 0.5], [0.5, -0.5], [-0.5, -0.5], [-0.5, 0.5]]");
geometry_msgs::Pose2D pose;
robot_geometry_msgs::Pose2D pose;
Polygon2D square2 = robot_nav_2d_utils::movePolygonToPose(square, pose);
EXPECT_TRUE(robot_nav_2d_utils::equals(square, square2));
pose.x = 15;

0
src/Libraries/nav_2d_utils/test/resolution_test.cpp → src/Libraries/robot_nav_2d_utils/test/resolution_test.cpp
View File

2
src/Libraries/tf3/CHANGELOG.rst
View File

@ -489,7 +489,7 @@ Changelog for package tf2
* more compatability
* bringing in helper functions for buffer_core from tf.h/cpp
* rethrowing to new exceptions
* converting Storage to geometry_msgs::TransformStamped
* converting Storage to robot_geometry_msgs::TransformStamped
* removing deprecated useage
* cleaning up includes
* moving all implementations into cpp file

4
src/Libraries/tf3/include/tf3/buffer_core.h
View File

@ -163,7 +163,7 @@ public:
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
robot_geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
const tf::Point & reference_point, const std::string& reference_point_frame,
const robot::Time& time, const robot::Duration& averaging_interval) const;
@ -181,7 +181,7 @@ public:
* New in geometry 1.1
*/
/*
geometry_msgs::Twist
robot_geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
const robot::Time& time, const robot::Duration& averaging_interval) const;
*/

4
src/Libraries/tf3/include/tf3/compat.h
View File

@ -1,4 +1,4 @@
// Compatibility types to avoid using robot:: or geometry_msgs:: namespaces inside tf3
// Compatibility types to avoid using robot:: or robot_geometry_msgs:: namespaces inside tf3
#ifndef TF3_COMPAT_H
#define TF3_COMPAT_H
@ -7,7 +7,7 @@
namespace tf3 {
// Minimal header/message equivalents owned by tf3 (no robot:: or geometry_msgs::)
// Minimal header/message equivalents owned by tf3 (no robot:: or robot_geometry_msgs::)
struct HeaderMsg
{
uint32_t seq;

2
src/Libraries/tf3/include/tf3/impl/utils.h
View File

@ -53,7 +53,7 @@ template<typename T>
}
/** Generic version of toQuaternion. It tries to convert the argument
* to a geometry_msgs::Quaternion
* to a robot_geometry_msgs::Quaternion
* \param t some object
* \return a copy of the same quaternion as a tf3::Quaternion
*/

8
src/Libraries/tf3/src/buffer_core.cpp
View File

@ -672,14 +672,14 @@ tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_f
/*
geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& observation_frame,
const robot::Time& time,
const robot::Duration& averaging_interval) const
{
try
{
geometry_msgs::Twist t;
robot_geometry_msgs::Twist t;
old_tf_.lookupTwist(tracking_frame, observation_frame,
time, averaging_interval, t);
return t;
@ -702,7 +702,7 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
}
}
geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
robot_geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& observation_frame,
const std::string& reference_frame,
const tf3::Point & reference_point,
@ -711,7 +711,7 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const robot::Duration& averaging_interval) const
{
try{
geometry_msgs::Twist t;
robot_geometry_msgs::Twist t;
old_tf_.lookupTwist(tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame,
time, averaging_interval, t);
return t;

2
src/Libraries/tf3/test/cache_unittest.cpp
View File

@ -32,7 +32,7 @@
#include "tf3/LinearMath/Quaternion.h"
#include <stdexcept>
#include <geometry_msgs/TransformStamped.h>
#include <robot_geometry_msgs/TransformStamped.h>
#include <cmath>

16
src/Libraries/tf3/test/simple_tf3_core.cpp
View File

@ -36,7 +36,7 @@
TEST(tf3, setTransformFail)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
EXPECT_FALSE(tfc.setTransform(st, "authority1"));
}
@ -44,7 +44,7 @@ TEST(tf3, setTransformFail)
TEST(tf3, setTransformValid)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
@ -56,7 +56,7 @@ TEST(tf3, setTransformValid)
TEST(tf3, setTransformInvalidQuaternion)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
@ -86,7 +86,7 @@ TEST(tf3_canTransform, Nothing_Exists)
TEST(tf3_lookupTransform, LookupException_One_Exists)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
@ -99,7 +99,7 @@ TEST(tf3_lookupTransform, LookupException_One_Exists)
TEST(tf3_canTransform, One_Exists)
{
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
@ -113,7 +113,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;
@ -196,7 +196,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;
@ -243,7 +243,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
tf3::BufferCore mBC;
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
robot_geometry_msgs::TransformStamped st;
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;

4
src/Libraries/tf3/test/speed_test.cpp
View File

@ -50,7 +50,7 @@ int main(int argc, char** argv)
console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_INFO);
tf3::BufferCore bc;
geometry_msgs::TransformStamped t;
robot_geometry_msgs::TransformStamped t;
t.header.stamp = robot::Time(1);
t.header.frame_id = "root";
t.child_frame_id = "0";
@ -106,7 +106,7 @@ int main(int argc, char** argv)
std::string v_frame0 = boost::lexical_cast<std::string>(num_levels - 1);
std::string v_frame1 = boost::lexical_cast<std::string>(num_levels/2 - 1);
CONSOLE_BRIDGE_logInform("%s to %s", v_frame0.c_str(), v_frame1.c_str());
geometry_msgs::TransformStamped out_t;
robot_geometry_msgs::TransformStamped out_t;
const uint32_t count = 1000000;
CONSOLE_BRIDGE_logInform("Doing %d %d-level %lf-interval tests", count, num_levels, time_interval);

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