update
This commit is contained in:
parent
dad2726eb1
commit
82afc4c203
6
.gitmodules
vendored
6
.gitmodules
vendored
|
|
@ -1,9 +1,9 @@
|
|||
[submodule "src/Libraries/common_msgs"]
|
||||
path = src/Libraries/common_msgs
|
||||
url = https://git.pnkr.asia/DuongTD/common_msgs.git
|
||||
[submodule "src/Libraries/costmap_2d"]
|
||||
path = src/Libraries/costmap_2d
|
||||
url = https://git.pnkr.asia/DuongTD/costmap_2d.git
|
||||
[submodule "src/Libraries/robot_costmap_2d"]
|
||||
path = src/Libraries/robot_costmap_2d
|
||||
url = https://git.pnkr.asia/DuongTD/robot_costmap_2d.git
|
||||
[submodule "src/Libraries/geometry2"]
|
||||
path = src/Libraries/geometry2
|
||||
url = https://git.pnkr.asia/DuongTD/geometry2.git
|
||||
|
|
|
|||
|
|
@ -67,8 +67,8 @@ flowchart TB
|
|||
%% ========== COSTMAP LAYER ==========
|
||||
subgraph Costmap["🗺️ Costmap Layer"]
|
||||
direction LR
|
||||
GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
|
||||
LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
|
||||
GC["<b>🌍 Global Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>🌍 frame: map<br/>━━━━━━━━━━━━━━━━<br/>🗺️ Static Map<br/>🚫 Obstacles<br/>💰 Inflation Layer"]
|
||||
LC["<b>📍 Local Costmap</b><br/>━━━━━━━━━━━━━━━━<br/>📦 robot_costmap_2d::Costmap2DROBOT<br/>📍 frame: odom<br/>━━━━━━━━━━━━━━━━<br/>🔍 Dynamic Obstacles<br/>📡 Sensor Fusion<br/>⚡ Real-time Updates"]
|
||||
style Costmap fill:#F1F8E9,stroke:#558B2F,stroke-width:4px,color:#000
|
||||
style GC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
|
||||
style LC fill:#DCEDC8,stroke:#558B2F,stroke-width:3px,font-size:13px
|
||||
|
|
@ -211,7 +211,7 @@ Cần làm rõ:
|
|||
- Plugin system sử dụng `boost::dll` để dynamic loading
|
||||
|
||||
5. **Costmap Layer** ✅
|
||||
- `costmap_2d::Costmap2DROBOT`: Global và local costmap
|
||||
- `robot_costmap_2d::Costmap2DROBOT`: Global và local costmap
|
||||
- Costmap layers: static map, obstacles, inflation
|
||||
- Frame management: map (global), odom (local)
|
||||
|
||||
|
|
@ -235,7 +235,7 @@ Cần làm rõ:
|
|||
- `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í)
|
||||
- `robot_costmap_2d::Costmap2D` (bản đồ chi phí)
|
||||
|
||||
### 3. Thiết kế từng module (interface level)
|
||||
|
||||
|
|
@ -264,7 +264,7 @@ Cần làm rõ:
|
|||
- `nav_core::RecoveryBehavior` ✅
|
||||
- `runBehavior()` - Thực thi recovery behavior
|
||||
|
||||
- `costmap_2d::Costmap2DROBOT` ✅
|
||||
- `robot_costmap_2d::Costmap2DROBOT` ✅
|
||||
- Wrapper cho costmap với robot footprint
|
||||
- Thread-safe access với mutex
|
||||
|
||||
|
|
@ -411,7 +411,7 @@ pnkx_nav_core/
|
|||
│ │ │ ├── nav_core_adapter/ # Adapter utilities
|
||||
│ │ │ └── nav_core2/ # Additional nav utilities
|
||||
│ │ ├── Libraries/
|
||||
│ │ │ ├── costmap_2d/ # Costmap system
|
||||
│ │ │ ├── robot_costmap_2d/ # Costmap system
|
||||
│ │ │ ├── tf3/ # Transform system
|
||||
│ │ │ ├── robot_time/ # Time management
|
||||
│ │ │ ├── geometry2/ # Geometry utilities
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@
|
|||
- Plugin system với `boost::dll` để load dynamic plugins
|
||||
|
||||
3. **Costmap System:**
|
||||
- `costmap_2d::Costmap2DROBOT` - Global và local costmap
|
||||
- `robot_costmap_2d::Costmap2DROBOT` - Global và local costmap
|
||||
- Costmap layers (static, obstacle, inflation, etc.)
|
||||
|
||||
4. **Supporting Libraries:**
|
||||
|
|
|
|||
|
|
@ -85,9 +85,9 @@ cp ../../build/src/APIs/c_api/libnav_c_api.so .
|
|||
|
||||
# 5. Set library path (quan trọng!)
|
||||
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
|
||||
|
|
@ -118,9 +118,9 @@ ls -la ../../build/src/APIs/c_api/libnav_c_api.so
|
|||
|
||||
# Set LD_LIBRARY_PATH (bao gồm tất cả dependencies)
|
||||
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
|
||||
|
|
@ -143,6 +143,6 @@ ldd libnav_c_api.so
|
|||
→ Đã được sửa! `navigation_create()` bây giờ tạo instance của `move_base::MoveBase`.
|
||||
Nếu vẫn gặp lỗi, kiểm tra:
|
||||
- Library đã được build đầy đủ: `make nav_c_api` trong build directory
|
||||
- Dependencies đã được build: `make move_base costmap_2d`
|
||||
- Dependencies đã được build: `make move_base robot_costmap_2d`
|
||||
- LD_LIBRARY_PATH đã được set đúng
|
||||
|
||||
|
|
|
|||
|
|
@ -78,9 +78,9 @@ Tạo file `NavigationExample.csproj`:
|
|||
```bash
|
||||
# Set LD_LIBRARY_PATH để tìm được tất cả dependencies
|
||||
export LD_LIBRARY_PATH=../../build/src/APIs/c_api:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH
|
||||
export LD_LIBRARY_PATH=../../build/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH
|
||||
|
|
|
|||
|
|
@ -95,9 +95,9 @@ export LD_LIBRARY_PATH="$BUILD_DIR:$LD_LIBRARY_PATH"
|
|||
# Library directories for dependencies
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_time:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/xmlrpcpp:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/costmap_2d:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/robot_costmap_2d:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_geometry_msgs:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Libraries/geometry2/robot_tf3_sensor_msgs:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Packages/move_base:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Cores/move_base_core:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/tf3:$LD_LIBRARY_PATH"
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
#include <nav_core2/exceptions.h>
|
||||
#include <nav_core2/costmap.h>
|
||||
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_geometry_msgs/PoseArray.h>
|
||||
#include <mkt_msgs/Trajectory2D.h>
|
||||
#include <score_algorithm/trajectory_generator.h>
|
||||
|
|
@ -38,7 +38,7 @@ namespace score_algorithm
|
|||
* @param tf TFListener pointer
|
||||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
virtual void initialize(robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
|
||||
|
||||
/**
|
||||
|
|
@ -144,7 +144,7 @@ namespace score_algorithm
|
|||
|
||||
std::string name_;
|
||||
TFListenerPtr tf_;
|
||||
costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
TrajectoryGenerator::Ptr traj_;
|
||||
|
||||
int index_samples_;
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ set(PACKAGES_DIR
|
|||
kalman
|
||||
angles
|
||||
nav_grid
|
||||
costmap_2d
|
||||
robot_costmap_2d
|
||||
robot_sensor_msgs
|
||||
robot_std_msgs
|
||||
)
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ namespace mkt_algorithm
|
|||
* @param nh NodeHandle to read parameters from
|
||||
*/
|
||||
virtual void initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
|
||||
/**
|
||||
* @brief Calculating algorithm
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ namespace mkt_algorithm
|
|||
* @param nh NodeHandle to read parameters from
|
||||
*/
|
||||
virtual void initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
|
||||
/**
|
||||
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
|
||||
|
|
@ -157,7 +157,7 @@ namespace mkt_algorithm
|
|||
*/
|
||||
bool transformGlobalPlan(
|
||||
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
|
||||
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
|
||||
robot_nav_2d_msgs::Path2D &transformed_plan);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@ namespace mkt_algorithm
|
|||
* @param nh NodeHandle to read parameters from
|
||||
*/
|
||||
virtual void initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
|
||||
|
||||
/**
|
||||
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
#include <robot/console.h>
|
||||
|
||||
void mkt_algorithm::diff::GoStraight::initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
#define LIMIT_VEL_THETA 0.33
|
||||
|
||||
void mkt_algorithm::diff::PredictiveTrajectory::initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
@ -544,10 +544,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
|
|||
kf_->update(y, dt, A);
|
||||
|
||||
// Check if Kalman filter's estimated velocity exceeds v_max
|
||||
if (avoid_obstacles_ && (cost_left_side_ >= costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_right_side_ >= costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE))
|
||||
if (avoid_obstacles_ && (cost_left_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_right_side_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_left_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE ||
|
||||
cost_right_goal_ >= robot_costmap_2d::UNPREFERRED_SPACE))
|
||||
{
|
||||
drive_cmd.x = fabs(kf_->state()[0]) > fabs(v_max) ? drive_cmd.x : kf_->state()[0];
|
||||
}
|
||||
|
|
@ -826,7 +826,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
|
|||
|
||||
bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
|
||||
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
|
||||
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
|
||||
robot_nav_2d_msgs::Path2D &transformed_plan)
|
||||
{
|
||||
// this method is a slightly modified version of base_local_planner/goal_functions.h
|
||||
|
|
@ -1029,12 +1029,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
|
|||
// ss << linear_vel << " ";
|
||||
// limit the linear velocity by proximity to obstacles
|
||||
if (use_cost_regulated_linear_velocity_scaling_ &&
|
||||
pose_cost != static_cast<double>(costmap_2d::NO_INFORMATION) &&
|
||||
pose_cost != static_cast<double>(costmap_2d::FREE_SPACE))
|
||||
pose_cost != static_cast<double>(robot_costmap_2d::NO_INFORMATION) &&
|
||||
pose_cost != static_cast<double>(robot_costmap_2d::FREE_SPACE))
|
||||
{
|
||||
const double inscribed_radius = costmap_robot_->getLayeredCostmap()->getInscribedRadius();
|
||||
const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) *
|
||||
std::log(pose_cost / (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
|
||||
std::log(pose_cost / (robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)) +
|
||||
inscribed_radius;
|
||||
|
||||
if (min_distance_to_obstacle < cost_scaling_dist_)
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
#include <angles/angles.h>
|
||||
|
||||
void mkt_algorithm::diff::RotateToGoal::initialize(
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit d6512018efc5ef63d64a6aeb97ecaf89d83cbd80
|
||||
Subproject commit 60e9c5673f8fd646d628f843ef73e71d1d9b2a17
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit 9c84e64253fe959931cd456cf2eb164af9ee1c92
|
||||
Subproject commit b5a1b7b6d8367b5812b32f2ca51bbed6e2a4c99a
|
||||
|
|
@ -27,13 +27,13 @@ file(GLOB HEADERS "include/two_points_planner/*.h")
|
|||
|
||||
# Dependencies packages (internal libraries)
|
||||
set(PACKAGES_DIR
|
||||
costmap_2d
|
||||
robot_costmap_2d
|
||||
nav_core
|
||||
geometry_msgs
|
||||
robot_nav_msgs
|
||||
robot_std_msgs
|
||||
tf3
|
||||
tf3_geometry_msgs
|
||||
robot_tf3_geometry_msgs
|
||||
)
|
||||
|
||||
# Tạo thư viện shared (.so)
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include <vector>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <nav_core/base_global_planner.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot/node_handle.h>
|
||||
|
||||
namespace two_points_planner
|
||||
|
|
@ -31,7 +31,7 @@ public:
|
|||
* @param name The name of this planner
|
||||
* @param costmap_robot A pointer to the ROS wrapper of the costmap to use
|
||||
*/
|
||||
TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
|
||||
/**
|
||||
* @brief Initialization function for the TwoPointsPlanner object
|
||||
|
|
@ -39,7 +39,7 @@ public:
|
|||
* @param costmap_robot A pointer to the ROS wrapper of the costmap to use
|
||||
*/
|
||||
virtual bool initialize(std::string name,
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
|
||||
/**
|
||||
* @brief Given a goal pose in the world, compute a plan
|
||||
|
|
@ -71,7 +71,7 @@ protected:
|
|||
bool allow_unknown_;
|
||||
|
||||
std::string name_;
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_robot_; /**< manages the cost map for us */
|
||||
std::vector<robot_geometry_msgs::Point> footprint_;
|
||||
unsigned int current_env_width_;
|
||||
unsigned int current_env_height_;
|
||||
|
|
|
|||
|
|
@ -1,11 +1,11 @@
|
|||
#include <robot/console.h>
|
||||
#include <two_points_planner/two_points_planner.h>
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
#include <robot_costmap_2d/inflation_layer.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <robot_nav_msgs/Path.h>
|
||||
#include <tf3/LinearMath/Quaternion.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot/time.h>
|
||||
#include <robot/console.h>
|
||||
#include <cmath>
|
||||
|
|
@ -15,13 +15,13 @@ namespace two_points_planner
|
|||
{
|
||||
TwoPointsPlanner::TwoPointsPlanner() : initialized_(false), costmap_robot_(NULL) {}
|
||||
|
||||
TwoPointsPlanner::TwoPointsPlanner(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
TwoPointsPlanner::TwoPointsPlanner(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
: initialized_(false), costmap_robot_(NULL)
|
||||
{
|
||||
initialize(name, costmap_robot);
|
||||
}
|
||||
|
||||
bool TwoPointsPlanner::initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
bool TwoPointsPlanner::initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
@ -66,11 +66,11 @@ namespace two_points_planner
|
|||
|
||||
// Bug
|
||||
// // check if the costmap has an inflation layer
|
||||
// for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
|
||||
// for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = costmap_robot_->getLayeredCostmap()->getPlugins()->begin();
|
||||
// layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
|
||||
// ++layer)
|
||||
// {
|
||||
// boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
|
||||
// boost::shared_ptr<robot_costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<robot_costmap_2d::InflationLayer>(*layer);
|
||||
// if (!inflation_layer)
|
||||
// continue;
|
||||
|
||||
|
|
@ -83,14 +83,14 @@ namespace two_points_planner
|
|||
// This rescales the costmap according to a rosparam which sets the obstacle cost
|
||||
unsigned char TwoPointsPlanner::costMapCostToSBPLCost(unsigned char newcost)
|
||||
{
|
||||
if (newcost == costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == costmap_2d::NO_INFORMATION))
|
||||
if (newcost == robot_costmap_2d::LETHAL_OBSTACLE || (!allow_unknown_ && newcost == robot_costmap_2d::NO_INFORMATION))
|
||||
return lethal_obstacle_;
|
||||
else if (newcost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
else if (newcost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
return inscribed_inflated_obstacle_;
|
||||
else if (newcost == 0)
|
||||
return 0;
|
||||
else if (newcost == costmap_2d::NO_INFORMATION)
|
||||
return costmap_2d::FREE_SPACE;
|
||||
else if (newcost == robot_costmap_2d::NO_INFORMATION)
|
||||
return robot_costmap_2d::FREE_SPACE;
|
||||
else
|
||||
{
|
||||
unsigned char cost = newcost;
|
||||
|
|
@ -151,8 +151,8 @@ namespace two_points_planner
|
|||
}
|
||||
unsigned char start_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_start, my_start));
|
||||
unsigned char end_cost = costMapCostToSBPLCost(costmap_robot_->getCostmap()->getCost(mx_end, my_end));
|
||||
if (start_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
|| end_cost == costMapCostToSBPLCost(costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
|
||||
if (start_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || start_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
|
||||
|| end_cost == costMapCostToSBPLCost(robot_costmap_2d::LETHAL_OBSTACLE) || end_cost == costMapCostToSBPLCost(robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE))
|
||||
{
|
||||
std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -37,10 +37,10 @@ set(PACKAGES_DIR
|
|||
nav_core2
|
||||
mkt_msgs
|
||||
score_algorithm
|
||||
costmap_2d
|
||||
robot_costmap_2d
|
||||
tf3
|
||||
tf3_geometry_msgs
|
||||
tf3_sensor_msgs
|
||||
robot_tf3_geometry_msgs
|
||||
robot_tf3_sensor_msgs
|
||||
robot_cpp
|
||||
angles
|
||||
)
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ namespace pnkx_local_planner
|
|||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
void initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
|
||||
|
|
@ -110,7 +110,7 @@ namespace pnkx_local_planner
|
|||
* @param tf TFListener pointer
|
||||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
void initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
void initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
score_algorithm::TrajectoryGenerator::Ptr traj_generator);
|
||||
|
||||
bool getMakerGoal(robot_nav_2d_msgs::Pose2DStamped &maker_goal);
|
||||
|
|
@ -156,7 +156,7 @@ namespace pnkx_local_planner
|
|||
|
||||
robot::NodeHandle nh_, nh_priv_;
|
||||
TFListenerPtr tf_;
|
||||
costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
|
||||
std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
|
||||
std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ namespace pnkx_local_planner
|
|||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
void initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@ namespace pnkx_local_planner
|
|||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
void initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 setGoalPose - Sets the global goal pose
|
||||
|
|
@ -183,8 +183,8 @@ namespace pnkx_local_planner
|
|||
robot_nav_2d_msgs::Path2D transformed_plan_;
|
||||
robot_nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
|
||||
|
||||
costmap_2d::Costmap2D *costmap_;
|
||||
costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
robot_costmap_2d::Costmap2D *costmap_;
|
||||
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
nav_grid::NavGridInfo info_;
|
||||
boost::recursive_mutex configuration_mutex_;
|
||||
bool update_costmap_before_planning_;
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ namespace pnkx_local_planner
|
|||
* @param costmap_robot Costmap pointer
|
||||
*/
|
||||
void initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
|
||||
|
|
|
|||
|
|
@ -11,9 +11,9 @@
|
|||
#include <tf3/buffer_core.h>
|
||||
#include <tf3/convert.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
|
||||
namespace pnkx_local_planner
|
||||
{
|
||||
|
|
@ -42,7 +42,7 @@ namespace pnkx_local_planner
|
|||
*/
|
||||
bool transformGlobalPlan(
|
||||
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D &global_plan, const robot_nav_2d_msgs::Pose2DStamped &pose,
|
||||
const costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
|
||||
const robot_costmap_2d::Costmap2DROBOT *costmap, const std::string &global_frame, double max_plan_length,
|
||||
robot_nav_2d_msgs::Path2D &transformed_plan, const bool from_global_frame = false);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::~PNKXDockingLocalPlanner()
|
|||
}
|
||||
|
||||
void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
@ -572,7 +572,7 @@ pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::~DockingPlanner()
|
|||
docking_nav_.reset();
|
||||
}
|
||||
|
||||
void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(robot::NodeHandle &nh, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot,
|
||||
score_algorithm::TrajectoryGenerator::Ptr traj_generator)
|
||||
{
|
||||
nh_ = nh;
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ pnkx_local_planner::PNKXGoStraightLocalPlanner::~PNKXGoStraightLocalPlanner()
|
|||
goal_checker_.reset();
|
||||
}
|
||||
|
||||
void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@
|
|||
|
||||
#include <tf3/convert.h>
|
||||
#include <tf3/utils.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
|
||||
#include <mkt_msgs/Trajectory2D.h>
|
||||
#include <boost/dll/import.hpp>
|
||||
|
|
@ -37,7 +37,7 @@ pnkx_local_planner::PNKXLocalPlanner::~PNKXLocalPlanner()
|
|||
}
|
||||
|
||||
void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ pnkx_local_planner::PNKXRotateLocalPlanner::~PNKXRotateLocalPlanner()
|
|||
goal_checker_.reset();
|
||||
}
|
||||
|
||||
void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <robot/console.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
|
||||
bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, std::string global_frame, robot_nav_2d_msgs::Pose2DStamped& global_pose, double transform_tolerance)
|
||||
{
|
||||
|
|
@ -89,7 +89,7 @@ double pnkx_local_planner::getSquareDistance(const robot_geometry_msgs::Pose2D&
|
|||
|
||||
bool pnkx_local_planner::transformGlobalPlan(
|
||||
TFListenerPtr tf, const robot_nav_2d_msgs::Path2D& global_plan, const robot_nav_2d_msgs::Pose2DStamped& pose,
|
||||
const costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
|
||||
const robot_costmap_2d::Costmap2DROBOT* costmap, const std::string& global_frame, double max_plan_length,
|
||||
robot_nav_2d_msgs::Path2D& transformed_plan, const bool from_global_frame)
|
||||
{
|
||||
if (global_plan.poses.size() == 0)
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 41d47c9c9e7116d837c16930151eb58165039a5b
|
||||
Subproject commit 0e486607b7868c37d3c40fed0207435a2e185171
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit 4246453ae6598b75be664acef3e9fc276129a131
|
||||
Subproject commit 72b2f3c6393d68ea9d3905fb569986520fa4538d
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit b1aaa1a9463853ec16574b27182c5753c630a407
|
||||
Subproject commit 594f0fe49e40273a7e2e74e44f0d63fa4fe2cfea
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit e4db1da907b8d77ac8b838d4a03e4e57a8c0eb6f
|
||||
Subproject commit 5558086d10e402e7afc605c6c68074ab4f530b9f
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit cb9dfe5c9be8d99a8b14dd0718a2da986aca5ef4
|
||||
Subproject commit fc6eb0c43fb2da4007529187e7328ed93f1fce66
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
# robot_nav_2d_utils Polygons and Footprints
|
||||
This library represents a replacement for [costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
|
||||
This library represents a replacement for [robot_costmap_2d/footprint.h](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/footprint.h) and deals with manipulating polygons. Note that implicitly all polygons here are assumed to be [simple polygons](https://en.wikipedia.org/wiki/Simple_polygon) without "holes."
|
||||
|
||||
## Polygons and the Parameter Server
|
||||
There have historically been three primary ways to specify a polygon/footprint on the parameter server. The first is to simply specify a radius which is converted to a hexadecagon, i.e. polygon with sixteen sides. This can be read with
|
||||
|
|
@ -21,7 +21,7 @@ robot_nav_2d_msgs::Polygon2D polygonFromXMLRPC(XmlRpc::XmlRpcValue& polygon_xmlr
|
|||
|
||||
If the `XmlRpcValue` is a string, it will call the `polygonFromString` method.
|
||||
|
||||
The above are the traditional methods that were supported by the original `costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
|
||||
The above are the traditional methods that were supported by the original `robot_costmap_2d` code. However, we add a fourth method that requires two parallel arrays of x and y coordinates.
|
||||
|
||||
```
|
||||
robot_nav_2d_msgs::Polygon2D polygonFromParallelArrays(const std::vector<double>& xs, const std::vector<double>& ys);
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@ namespace robot_nav_2d_utils
|
|||
/**
|
||||
* @brief Load the robot footprint either as a polygon from the footprint parameter or from robot_radius
|
||||
*
|
||||
* Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
|
||||
* Analagous to robot_costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
|
||||
* is present.
|
||||
*/
|
||||
robot_nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle &nh, bool write = true);
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
#include <robot_nav_2d_utils/tf_help.h>
|
||||
#include <robot_nav_2d_msgs/Pose2DStamped.h>
|
||||
#include <robot_nav_2d_utils/conversions.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <string>
|
||||
|
||||
namespace robot_nav_2d_utils
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@ include_directories(
|
|||
|
||||
# Tạo INTERFACE library (header-only)
|
||||
add_library(nav_core INTERFACE)
|
||||
target_link_libraries(nav_core INTERFACE costmap_2d tf3 robot_protocol_msgs)
|
||||
target_link_libraries(nav_core INTERFACE robot_costmap_2d tf3 robot_protocol_msgs)
|
||||
|
||||
# Set include directories
|
||||
target_include_directories(nav_core
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
#define NAV_CORE_BASE_GLOBAL_PLANNER_H
|
||||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
#include <memory>
|
||||
|
||||
|
|
@ -97,7 +97,7 @@ namespace nav_core {
|
|||
* @param name The name of this planner
|
||||
* @param costmap_robot A pointer to the wrapper of the costmap to use for planning
|
||||
*/
|
||||
virtual bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
|
||||
virtual bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor for the interface
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@
|
|||
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
#include <memory>
|
||||
|
||||
|
|
@ -138,7 +138,7 @@ namespace nav_core
|
|||
* @param tf A pointer to a transform listener
|
||||
* @param costmap_robot The cost map to use for assigning costs to local plans
|
||||
*/
|
||||
virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
|
||||
virtual void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor for the interface
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@
|
|||
#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
|
||||
#define NAV_CORE_RECOVERY_BEHAVIOR_H
|
||||
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
#include <memory>
|
||||
|
||||
|
|
@ -57,7 +57,7 @@ namespace nav_core {
|
|||
* @param global_costmap A pointer to the global_costmap used by the navigation stack
|
||||
* @param local_costmap A pointer to the local_costmap used by the navigation stack
|
||||
*/
|
||||
virtual void initialize(std::string name, tf3::BufferCore* tf, costmap_2d::Costmap2DROBOT* global_costmap, costmap_2d::Costmap2DROBOT* local_costmap) = 0;
|
||||
virtual void initialize(std::string name, tf3::BufferCore* tf, robot_costmap_2d::Costmap2DROBOT* global_costmap, robot_costmap_2d::Costmap2DROBOT* local_costmap) = 0;
|
||||
|
||||
/**
|
||||
* @brief Runs the RecoveryBehavior
|
||||
|
|
|
|||
|
|
@ -24,7 +24,7 @@ file(GLOB HEADERS "include/nav_core2/*.h")
|
|||
add_library(nav_core2 INTERFACE)
|
||||
|
||||
target_link_libraries(nav_core2 INTERFACE
|
||||
costmap_2d
|
||||
robot_costmap_2d
|
||||
tf3
|
||||
nav_grid
|
||||
robot_nav_2d_msgs
|
||||
|
|
|
|||
|
|
@ -3,12 +3,12 @@ A replacement interface for [nav_core](https://github.com/ros-planning/navigatio
|
|||
|
||||
There were a few key reasons for creating new interfaces rather than extending the old ones.
|
||||
* Use `robot_nav_2d_msgs` to eliminate unused data fields
|
||||
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROBOT`.
|
||||
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `robot_costmap_2d::Costmap2DROBOT`.
|
||||
* Provide more data in the interfaces for easier testing.
|
||||
* Use Exceptions rather than booleans to provide more information about the types of errors encountered.
|
||||
|
||||
## `Costmap`
|
||||
`costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
|
||||
`robot_costmap_2d::Costmap2DROBOT` has been a vital part of the navigation stack for years, but it was not without its flaws.
|
||||
* Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROBOT`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
|
||||
* Initialization also started an update thread, which is also not always needed in testing.
|
||||
* Using `Costmap2DROBOT` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
|
||||
|
|
@ -29,7 +29,7 @@ Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-plan
|
|||
|
||||
| `nav_core` | `nav_core2` | comments |
|
||||
|---|--|---|
|
||||
|`void initialize(std::string, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|
||||
|`void initialize(std::string, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|
||||
|`bool makePlan(const robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
|
||||
|
||||
## Local Planner
|
||||
|
|
@ -37,7 +37,7 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
|
|||
|
||||
| `nav_core` | `nav_core2` | comments |
|
||||
|---|--|---|
|
||||
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|
||||
|`void initialize(std::string, tf::TransformListener*, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|
||||
|(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|
||||
|`bool setPlan(const std::vector<robot_geometry_msgs::PoseStamped>&)`|`setPlan(const robot_nav_2d_msgs::Path2D&)`||
|
||||
|`bool computeVelocityCommands(robot_geometry_msgs::Twist&)`|`robot_nav_2d_msgs::Twist2DStamped computeVelocityCommands(const robot_nav_2d_msgs::Pose2DStamped&, const robot_nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@
|
|||
#include <robot_nav_2d_msgs/Twist2DStamped.h>
|
||||
#include <robot_geometry_msgs/Twist.h>
|
||||
#include <string>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <memory>
|
||||
|
||||
namespace nav_core2
|
||||
|
|
@ -77,7 +77,7 @@ namespace nav_core2
|
|||
// TFListenerPtr tf, Costmap::Ptr costmap) = 0;
|
||||
|
||||
virtual void initialize(robot::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
|
||||
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap) = 0;
|
||||
|
||||
/**
|
||||
* @brief Sets the global goal for this local planner.
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ target_include_directories(global_planner_adapter PRIVATE
|
|||
# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
|
||||
set(_NAV_CORE_ADAPTER_RPATH
|
||||
"${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
|
||||
"${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d"
|
||||
"${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
|
||||
"${CMAKE_BINARY_DIR}/src/Libraries/tf3"
|
||||
"${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
|
||||
"${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ This package contains adapters for using `nav_core` plugins as `nav_core2` plugi
|
|||
* Converting between 2d and 3d datatypes.
|
||||
* Converting between returning false and throwing exceptions on failure.
|
||||
|
||||
We also provide an adapter for using a `costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
|
||||
We also provide an adapter for using a `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `nav_core2::Costmap` interface.
|
||||
|
||||
## Adapter Classes
|
||||
* Global Planner Adapters
|
||||
|
|
|
|||
|
|
@ -37,12 +37,12 @@
|
|||
|
||||
#include <nav_core2/common.h>
|
||||
#include <nav_core2/costmap.h>
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <string>
|
||||
|
||||
namespace nav_core_adapter
|
||||
{
|
||||
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot);
|
||||
|
||||
class CostmapAdapter : public nav_core2::Costmap
|
||||
{
|
||||
|
|
@ -57,7 +57,7 @@ public:
|
|||
* @param costmap_robot A Costmap2DROBOT object
|
||||
* @param needs_destruction Whether to free the costmap_robot object when this class is destroyed
|
||||
*/
|
||||
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
|
||||
void initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
|
||||
|
||||
// Standard Costmap Interface
|
||||
void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
|
||||
|
|
@ -72,11 +72,11 @@ public:
|
|||
void updateInfo(const nav_grid::NavGridInfo& new_info) override;
|
||||
|
||||
// Get Costmap Pointer for Backwards Compatibility
|
||||
costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
|
||||
robot_costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
|
||||
|
||||
protected:
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot_;
|
||||
costmap_2d::Costmap2D* costmap_;
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
|
||||
robot_costmap_2d::Costmap2D* costmap_;
|
||||
bool needs_destruction_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ protected:
|
|||
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
|
||||
nav_core::BaseGlobalPlanner::Ptr planner_;
|
||||
|
||||
costmap_2d::Costmap2DROBOT* costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
|
||||
nav_core2::Costmap::Ptr costmap_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -77,7 +77,7 @@ namespace nav_core_adapter
|
|||
virtual ~LocalPlannerAdapter();
|
||||
|
||||
// Standard ROS Local Planner Interface
|
||||
void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
void initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
|
||||
|
||||
/**
|
||||
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
|
||||
|
|
@ -193,7 +193,7 @@ namespace nav_core_adapter
|
|||
TFListenerPtr tf_;
|
||||
|
||||
std::shared_ptr<CostmapAdapter> costmap_adapter_;
|
||||
costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
|
||||
|
||||
boost::recursive_mutex configuration_mutex_;
|
||||
|
||||
|
|
|
|||
|
|
@ -43,10 +43,10 @@
|
|||
namespace nav_core_adapter
|
||||
{
|
||||
|
||||
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot)
|
||||
{
|
||||
nav_grid::NavGridInfo info;
|
||||
costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
|
||||
robot_costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
|
||||
info.width = costmap->getSizeInCellsX();
|
||||
info.height = costmap->getSizeInCellsY();
|
||||
info.resolution = costmap->getResolution();
|
||||
|
|
@ -64,7 +64,7 @@ CostmapAdapter::~CostmapAdapter()
|
|||
}
|
||||
}
|
||||
|
||||
void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
|
||||
void CostmapAdapter::initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
|
||||
{
|
||||
costmap_robot_ = costmap_robot;
|
||||
needs_destruction_ = needs_destruction;
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ namespace nav_core_adapter
|
|||
/**
|
||||
* @brief Load the nav_core2 local planner and initialize it
|
||||
*/
|
||||
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
|
||||
{
|
||||
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
|
||||
costmap_robot_ = costmap_robot;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
|
|||
|
||||
nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
|
||||
|
||||
costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
|
||||
robot_costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
|
||||
lpa->initialize("lpa", &tf, &costmap_robot);
|
||||
|
||||
delete lpa;
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav
|
|||
The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`.
|
||||
|
||||
## Coordinate Conversion
|
||||
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
|
||||
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`robot_costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_costmap_2d/include/robot_costmap_2d/robot_costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
|
||||
* `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell.
|
||||
* `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed.
|
||||
* `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`.
|
||||
|
|
|
|||
|
|
@ -91,7 +91,7 @@ inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx,
|
|||
/**
|
||||
* @brief Convert from world coordinates to grid coordinates
|
||||
*
|
||||
* Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds.
|
||||
* Combined functionality from robot_costmap_2d::worldToMap and robot_costmap_2d::worldToMapEnforceBounds.
|
||||
* The output parameters are set to grid indexes within the grid, even if the function returns false,
|
||||
* meaning the coordinates are outside the grid.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ namespace nav_grid
|
|||
{
|
||||
/**
|
||||
* @class NavGrid
|
||||
* This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that
|
||||
* This class is a spiritual successor to the robot_costmap_2d::Costmap2D class, with the key differences being that
|
||||
* the datatype and data storage methods are not specified, and the frame_id is specified.
|
||||
*
|
||||
* The templatized nature of the class allows you to store whatever you like at each grid location, including
|
||||
|
|
|
|||
|
|
@ -86,11 +86,11 @@ Ngược lại, sẽ build ở **Standalone CMake mode**.
|
|||
### Internal Packages (cần build trước):
|
||||
- move_base_core
|
||||
- nav_core
|
||||
- costmap_2d
|
||||
- robot_costmap_2d
|
||||
- xmlrpcpp
|
||||
- node_handle
|
||||
- tf3_sensor_msgs
|
||||
- tf3_geometry_msgs
|
||||
- robot_tf3_sensor_msgs
|
||||
- robot_tf3_geometry_msgs
|
||||
|
||||
### System Libraries:
|
||||
- Boost
|
||||
|
|
|
|||
|
|
@ -49,8 +49,8 @@ include_directories(
|
|||
# ========================================================
|
||||
set(CMAKE_SKIP_BUILD_RPATH FALSE)
|
||||
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
|
||||
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
|
||||
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
|
||||
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
|
||||
set(CMAKE_INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp")
|
||||
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
|
||||
|
||||
# ========================================================
|
||||
|
|
@ -68,11 +68,11 @@ set(PACKAGES_DIR
|
|||
robot_std_msgs
|
||||
move_base_core
|
||||
nav_core
|
||||
costmap_2d
|
||||
robot_costmap_2d
|
||||
plugins # Link với plugins library để có StaticLayer typeinfo
|
||||
yaml-cpp
|
||||
tf3_sensor_msgs
|
||||
tf3_geometry_msgs
|
||||
robot_tf3_sensor_msgs
|
||||
robot_tf3_geometry_msgs
|
||||
data_convert
|
||||
dl
|
||||
pthread
|
||||
|
|
|
|||
|
|
@ -24,8 +24,8 @@
|
|||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
#include <costmap_2d/costmap_2d_robot.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <robot_costmap_2d/costmap_2d_robot.h>
|
||||
#include <robot_costmap_2d/costmap_2d.h>
|
||||
#include <robot_nav_msgs/GetPlan.h>
|
||||
#include <robot_protocol_msgs/Order.h>
|
||||
|
||||
|
|
@ -293,7 +293,7 @@ namespace move_base
|
|||
|
||||
bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q);
|
||||
|
||||
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap);
|
||||
bool getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap);
|
||||
|
||||
double distance(const robot_geometry_msgs::PoseStamped &p1, const robot_geometry_msgs::PoseStamped &p2);
|
||||
|
||||
|
|
@ -317,8 +317,8 @@ namespace move_base
|
|||
nav_core::BaseGlobalPlanner::Ptr planner_;
|
||||
std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
|
||||
|
||||
costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
|
||||
costmap_2d::Costmap2DROBOT *planner_costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
|
||||
robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;
|
||||
std::string robot_base_frame_, global_frame_;
|
||||
std::vector<std::string> recovery_behavior_names_;
|
||||
unsigned int recovery_index_;
|
||||
|
|
|
|||
|
|
@ -9,15 +9,15 @@
|
|||
#include <stdexcept>
|
||||
#include <cstdlib>
|
||||
#include <data_convert/data_convert.h>
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/dll/import.hpp>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <robot/plugin_loader_helper.h>
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <costmap_2d/voxel_layer.h>
|
||||
#include <robot_costmap_2d/static_layer.h>
|
||||
#include <robot_costmap_2d/voxel_layer.h>
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_sensor_msgs/LaserScan.h>
|
||||
|
||||
|
|
@ -177,14 +177,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
|
||||
try
|
||||
{
|
||||
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
|
||||
planner_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
|
||||
planner_costmap_robot_->pause();
|
||||
costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
|
||||
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
|
||||
robot_costmap_2d::LayeredCostmap* layered_costmap_ = planner_costmap_robot_->getLayeredCostmap();
|
||||
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
|
||||
layer != layered_costmap_->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
boost::shared_ptr<costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<costmap_2d::StaticLayer>(*layer);
|
||||
boost::shared_ptr<robot_costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<robot_costmap_2d::StaticLayer>(*layer);
|
||||
if (!static_layer)
|
||||
continue;
|
||||
robot_nav_msgs::OccupancyGrid *occupancy_grid = new robot_nav_msgs::OccupancyGrid();
|
||||
|
|
@ -203,7 +203,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
occupancy_grid->data.resize(occupancy_grid->info.width * occupancy_grid->info.height);
|
||||
for (int i = 0; i < occupancy_grid->data.size(); i++)
|
||||
{
|
||||
occupancy_grid->data[i] = costmap_2d::NO_INFORMATION;
|
||||
occupancy_grid->data[i] = robot_costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
if (occupancy_grid)
|
||||
static_layer->dataCallBack<robot_nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
|
||||
|
|
@ -241,14 +241,14 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
|
|||
|
||||
try
|
||||
{
|
||||
controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
|
||||
controller_costmap_robot_ = new robot_costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
|
||||
controller_costmap_robot_->pause();
|
||||
costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
|
||||
for (std::vector<boost::shared_ptr<costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
|
||||
robot_costmap_2d::LayeredCostmap* layered_costmap_ = controller_costmap_robot_->getLayeredCostmap();
|
||||
for (std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>::const_iterator layer = layered_costmap_->getPlugins()->begin();
|
||||
layer != layered_costmap_->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
boost::shared_ptr<costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<costmap_2d::VoxelLayer>(*layer);
|
||||
boost::shared_ptr<robot_costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<robot_costmap_2d::VoxelLayer>(*layer);
|
||||
if (!obstacle_layer)
|
||||
continue;
|
||||
robot_sensor_msgs::LaserScan *laser_scan = new robot_sensor_msgs::LaserScan();
|
||||
|
|
@ -401,7 +401,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
|
||||
try
|
||||
{
|
||||
|
|
@ -496,7 +496,7 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
try
|
||||
{
|
||||
tc_->swapPlanner(docking_planner_name_);
|
||||
|
|
@ -559,7 +559,7 @@ bool move_base::MoveBase::moveStraightTo(const robot_geometry_msgs::PoseStamped
|
|||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
tc_->swapPlanner(go_straight_planner_name_);
|
||||
|
||||
if (nav_feedback_)
|
||||
|
|
@ -608,7 +608,7 @@ bool move_base::MoveBase::rotateTo(const robot_geometry_msgs::PoseStamped &goal,
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
tc_->swapPlanner(rotate_planner_name_);
|
||||
|
||||
if (nav_feedback_)
|
||||
|
|
@ -640,7 +640,7 @@ void move_base::MoveBase::pause()
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
if (!tc_)
|
||||
{
|
||||
throw std::runtime_error("Null 'tc_' pointer encountered");
|
||||
|
|
@ -668,7 +668,7 @@ void move_base::MoveBase::resume()
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
if (!tc_)
|
||||
{
|
||||
throw std::runtime_error("Null 'tc_' pointer encountered");
|
||||
|
|
@ -689,7 +689,7 @@ void move_base::MoveBase::cancel()
|
|||
{
|
||||
throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered");
|
||||
}
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_robot_->getCostmap()->getMutex()));
|
||||
cancel_ctr_ = true;
|
||||
}
|
||||
|
||||
|
|
@ -811,7 +811,7 @@ bool move_base::MoveBase::makePlan(const robot_geometry_msgs::PoseStamped &goal,
|
|||
return false;
|
||||
}
|
||||
|
||||
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
|
||||
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_robot_->getCostmap()->getMutex()));
|
||||
|
||||
// get the starting pose of the robot
|
||||
robot_geometry_msgs::PoseStamped global_pose;
|
||||
|
|
@ -1028,7 +1028,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
|
|||
pt.y = y + size_y / 2;
|
||||
clear_poly.push_back(pt);
|
||||
|
||||
planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
|
||||
planner_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
|
||||
|
||||
// clear the controller's costmap
|
||||
getRobotPose(global_pose, controller_costmap_robot_);
|
||||
|
|
@ -1053,7 +1053,7 @@ void move_base::MoveBase::clearCostmapWindows(double size_x, double size_y)
|
|||
pt.y = y + size_y / 2;
|
||||
clear_poly.push_back(pt);
|
||||
|
||||
controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
|
||||
controller_costmap_robot_->getCostmap()->setConvexPolygonCost(clear_poly, robot_costmap_2d::FREE_SPACE);
|
||||
}
|
||||
|
||||
void move_base::MoveBase::publishZeroVelocity()
|
||||
|
|
@ -1219,7 +1219,7 @@ bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternio
|
|||
return true;
|
||||
}
|
||||
|
||||
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROBOT *costmap)
|
||||
bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &global_pose, robot_costmap_2d::Costmap2DROBOT *costmap)
|
||||
{
|
||||
|
||||
tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user