test finish
This commit is contained in:
parent
6c4d21826b
commit
2fb4827c00
|
|
@ -2,7 +2,7 @@ global_costmap:
|
|||
frame_id: map
|
||||
plugins:
|
||||
- {name: navigation_map, type: "StaticLayer" }
|
||||
- {name: virtual_walls_map, type: "StaticLayer" }
|
||||
# - {name: virtual_walls_map, type: "StaticLayer" }
|
||||
- {name: obstacles, type: "VoxelLayer" }
|
||||
- {name: inflation, type: "InflationLayer" }
|
||||
obstacles:
|
||||
|
|
|
|||
|
|
@ -10,6 +10,8 @@ CustomPlanner:
|
|||
nominalvel_mpersecs: 0.8
|
||||
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
|
||||
allow_unknown: true
|
||||
|
||||
directory_to_save_paths: "/init/paths"
|
||||
pathway_filename: "pathway.txt"
|
||||
current_pose_topic_name: "/amcl_pose"
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -5,45 +5,49 @@ go_straight_planner_name: PNKXGoStraightLocalPlanner
|
|||
rotate_planner_name: PNKXRotateLocalPlanner
|
||||
|
||||
base_local_planner: LocalPlannerAdapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.03
|
||||
min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
stateful: true
|
||||
publish_topic: true
|
||||
|
||||
LocalPlannerAdapter:
|
||||
PNKXLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
library_path: liblocal_planner_adapter
|
||||
yaw_goal_tolerance: 0.017
|
||||
xy_goal_tolerance: 0.03
|
||||
min_approach_linear_velocity: 0.06 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
|
||||
|
||||
PNKXDockingLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
PNKXLocalPlanner:
|
||||
# Algorithm
|
||||
library_path: libpnkx_local_planner
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
|
||||
PNKXGoStraightLocalPlanner:
|
||||
# Algorithm
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffGoStraight
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
PNKXDockingLocalPlanner:
|
||||
# Algorithm
|
||||
library_path: libpnkx_local_planner
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffPredictiveTrajectory
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
|
||||
PNKXRotateLocalPlanner:
|
||||
# Algorithm
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
# Goal checking
|
||||
goal_checker_name: SimpleGoalChecker
|
||||
stateful: true
|
||||
PNKXGoStraightLocalPlanner:
|
||||
# Algorithm
|
||||
library_path: libpnkx_local_planner
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
algorithm_nav_name: MKTAlgorithmDiffGoStraight
|
||||
# Goal checking
|
||||
goal_checker_name: GoalChecker
|
||||
|
||||
PNKXRotateLocalPlanner:
|
||||
# Algorithm
|
||||
library_path: libpnkx_local_planner
|
||||
algorithm_rotate_name: MKTAlgorithmDiffRotateToGoal
|
||||
trajectory_generator_name: LimitedAccelGenerator
|
||||
# Goal checking
|
||||
goal_checker_name: SimpleGoalChecker
|
||||
stateful: true
|
||||
|
||||
LimitedAccelGenerator:
|
||||
library_path: libmkt_plugins_standard_traj_generator
|
||||
max_vel_x: 1.2
|
||||
min_vel_x: -1.2
|
||||
|
||||
|
|
@ -75,7 +79,8 @@ LimitedAccelGenerator:
|
|||
# sim_period
|
||||
include_last_point: true
|
||||
|
||||
PredictiveTrajectory:
|
||||
MKTAlgorithmDiffPredictiveTrajectory:
|
||||
library_path: libmkt_algorithm_diff
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
|
|
@ -115,7 +120,8 @@ PredictiveTrajectory:
|
|||
cost_scaling_dist: 0.2 # (default: 0.6)
|
||||
cost_scaling_gain: 2.0 # (default: 1.0)
|
||||
|
||||
GoStraight:
|
||||
MKTAlgorithmDiffGoStraight:
|
||||
library_path: libmkt_algorithm_diff
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
|
|
@ -145,7 +151,8 @@ GoStraight:
|
|||
use_regulated_linear_velocity_scaling: false
|
||||
use_cost_regulated_linear_velocity_scaling: false
|
||||
|
||||
RotateToGoal:
|
||||
MKTAlgorithmDiffRotateToGoal:
|
||||
library_path: libmkt_algorithm_diff
|
||||
avoid_obstacles: false
|
||||
xy_local_goal_tolerance: 0.01
|
||||
angle_threshold: 0.6
|
||||
|
|
@ -183,4 +190,11 @@ RotateToGoal:
|
|||
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
|
||||
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
|
||||
cost_scaling_dist: 0.2 # (default: 0.6)
|
||||
cost_scaling_gain: 2.0 # (default: 1.0)
|
||||
cost_scaling_gain: 2.0 # (default: 1.0)
|
||||
|
||||
|
||||
GoalChecker:
|
||||
library_path: libmkt_plugins_goal_checker
|
||||
|
||||
SimpleGoalChecker:
|
||||
library_path: libmkt_plugins_simple_goal_checker
|
||||
Binary file not shown.
|
|
@ -90,14 +90,21 @@ echo "Copying library..."
|
|||
cp "$LIB_DIR/libnav_c_api.so" .
|
||||
|
||||
# Bước 4: Set LD_LIBRARY_PATH để tìm được tất cả dependencies
|
||||
export LD_LIBRARY_PATH="$LIB_DIR:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/costmap_2d:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_geometry_msgs:$LD_LIBRARY_PATH"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/geometry2/tf3_sensor_msgs:$LD_LIBRARY_PATH"
|
||||
# Main build directory (contains libtf3.so, librobot_cpp.so, etc.)
|
||||
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/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"
|
||||
export LD_LIBRARY_PATH="$BUILD_DIR/src/Navigations/Libraries/robot_time:$LD_LIBRARY_PATH"
|
||||
# Also add the example directory where the library is copied
|
||||
export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample:$LD_LIBRARY_PATH"
|
||||
# Add the output directory where .NET places the library
|
||||
export LD_LIBRARY_PATH="$EXAMPLE_DIR/NavigationExample/bin/Debug/net6.0/linux-x64:$LD_LIBRARY_PATH"
|
||||
|
||||
# Bước 5: Build và chạy
|
||||
echo "Building C# project..."
|
||||
|
|
|
|||
|
|
@ -5,10 +5,6 @@
|
|||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_utils/path_ops.h>
|
||||
#include <nav_2d_utils/parameters.h>
|
||||
|
||||
// #include <g2o/core/base_binary_edge.h>
|
||||
// #include <g2o/core/base_unary_edge.h>
|
||||
// #include <g2o/core/base_multi_edge.h>
|
||||
#include <angles/angles.h>
|
||||
|
||||
#include <tf3/convert.h>
|
||||
|
|
@ -18,6 +14,7 @@
|
|||
#include <mkt_msgs/Trajectory2D.h>
|
||||
#include <boost/dll/import.hpp>
|
||||
#include <boost/dll/alias.hpp>
|
||||
#include <robot/plugin_loader_helper.h>
|
||||
|
||||
pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner()
|
||||
: initialized_(false)
|
||||
|
|
@ -61,7 +58,6 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
|
||||
parent_ = parent;
|
||||
planner_nh_ = robot::NodeHandle(parent_, name);
|
||||
// planner_nh_.printRootParams();
|
||||
this->getParams(planner_nh_);
|
||||
|
||||
std::string traj_generator_name;
|
||||
|
|
@ -69,7 +65,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
|
||||
try
|
||||
{
|
||||
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_standard_traj_generator.so";
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath(traj_generator_name);
|
||||
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
|
||||
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
|
||||
traj_generator_ = traj_gen_loader_();
|
||||
|
|
@ -93,7 +90,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
|
||||
try
|
||||
{
|
||||
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so";
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath(algorithm_nav_name);
|
||||
nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
|
||||
path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
|
||||
nav_algorithm_ = nav_algorithm_loader_();
|
||||
|
|
@ -114,7 +112,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("MKTAlgorithmDiffRotateToGoal"));
|
||||
try
|
||||
{
|
||||
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_algorithm/libmkt_algorithm_diff.so";
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath(algorithm_rotate_name);
|
||||
rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
|
||||
path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
|
||||
rotate_algorithm_ = rotate_algorithm_loader_();
|
||||
|
|
@ -136,7 +135,8 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
|
|||
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
|
||||
try
|
||||
{
|
||||
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Libraries/mkt_plugins/libmkt_plugins_goal_checker.so";
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath(goal_checker_name);
|
||||
goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
|
||||
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
|
||||
goal_checker_ = goal_checker_loader_();
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit 10521c1629301d3e81f040213f08de7881bc5e59
|
||||
Subproject commit 0344c31e5b4185da41bfb253db720d9c91b802bd
|
||||
|
|
@ -95,7 +95,8 @@ namespace nav_core_adapter
|
|||
}
|
||||
try
|
||||
{
|
||||
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath(planner_name);
|
||||
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
|
||||
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
|
||||
planner_ = planner_loader_();
|
||||
|
|
@ -105,7 +106,7 @@ namespace nav_core_adapter
|
|||
exit(EXIT_FAILURE);
|
||||
}
|
||||
robot::log_info("\nLocalPlannerAdapter initialize: %s", planner_name.c_str());
|
||||
planner_->initialize(adapter_nh_, planner_name, tf_, costmap_robot_);
|
||||
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
|
||||
robot::log_info("[%s:%d]\n Successfully initialized planner \"%s\"", __FILE__, __LINE__, planner_name.c_str());
|
||||
}
|
||||
catch (const boost::system::system_error &ex)
|
||||
|
|
|
|||
|
|
@ -117,6 +117,7 @@ if(BUILDING_WITH_CATKIN)
|
|||
move_base_core
|
||||
nav_core
|
||||
costmap_2d
|
||||
plugins # Link với plugins library để có StaticLayer typeinfo
|
||||
xmlrpcpp
|
||||
robot_cpp
|
||||
tf3_sensor_msgs
|
||||
|
|
@ -139,6 +140,7 @@ else()
|
|||
move_base_core
|
||||
nav_core
|
||||
costmap_2d
|
||||
plugins # Link với plugins library để có StaticLayer typeinfo
|
||||
yaml-cpp
|
||||
xmlrpcpp
|
||||
tf3_sensor_msgs
|
||||
|
|
|
|||
|
|
@ -17,6 +17,11 @@
|
|||
#include <boost/dll/alias.hpp>
|
||||
#include <robot/plugin_loader_helper.h>
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <costmap_2d/voxel_layer.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
|
||||
move_base::MoveBase::MoveBase()
|
||||
: initialized_(false),
|
||||
planner_costmap_robot_(NULL), controller_costmap_robot_(NULL),
|
||||
|
|
@ -175,6 +180,35 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
|
|||
{
|
||||
planner_costmap_robot_ = new 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();
|
||||
layer != layered_costmap_->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
boost::shared_ptr<costmap_2d::StaticLayer> static_layer = boost::dynamic_pointer_cast<costmap_2d::StaticLayer>(*layer);
|
||||
if (!static_layer)
|
||||
continue;
|
||||
nav_msgs::OccupancyGrid *occupancy_grid = new nav_msgs::OccupancyGrid();
|
||||
occupancy_grid->header.frame_id = "map";
|
||||
occupancy_grid->header.stamp = robot::Time::now();
|
||||
occupancy_grid->info.resolution = 0.05;
|
||||
occupancy_grid->info.width = 100;
|
||||
occupancy_grid->info.height = 100;
|
||||
occupancy_grid->info.origin.position.x = 0.0;
|
||||
occupancy_grid->info.origin.position.y = 0.0;
|
||||
occupancy_grid->info.origin.position.z = 0.0;
|
||||
occupancy_grid->info.origin.orientation.x = 0.0;
|
||||
occupancy_grid->info.origin.orientation.y = 0.0;
|
||||
occupancy_grid->info.origin.orientation.z = 0.0;
|
||||
occupancy_grid->info.origin.orientation.w = 1.0;
|
||||
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;
|
||||
}
|
||||
if (occupancy_grid)
|
||||
static_layer->dataCallBack<nav_msgs::OccupancyGrid>(*occupancy_grid, "map");
|
||||
}
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
|
|
@ -210,6 +244,32 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
|
|||
{
|
||||
controller_costmap_robot_ = new 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();
|
||||
layer != layered_costmap_->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
boost::shared_ptr<costmap_2d::VoxelLayer> obstacle_layer = boost::dynamic_pointer_cast<costmap_2d::VoxelLayer>(*layer);
|
||||
if (!obstacle_layer)
|
||||
continue;
|
||||
sensor_msgs::LaserScan *laser_scan = new sensor_msgs::LaserScan();
|
||||
laser_scan->header.frame_id = "laser";
|
||||
laser_scan->header.stamp = robot::Time::now();
|
||||
laser_scan->angle_min = -M_PI;
|
||||
laser_scan->angle_max = M_PI;
|
||||
laser_scan->angle_increment = M_PI / 180;
|
||||
laser_scan->time_increment = 0.0;
|
||||
laser_scan->scan_time = 0.0;
|
||||
laser_scan->range_min = 0.0;
|
||||
laser_scan->range_max = 10.0;
|
||||
laser_scan->ranges.resize(180);
|
||||
for (int i = 0; i < 180; i++)
|
||||
{
|
||||
laser_scan->ranges[i] = 10.0;
|
||||
}
|
||||
if (laser_scan)
|
||||
obstacle_layer->dataCallBack<sensor_msgs::LaserScan>(*laser_scan, "laser");
|
||||
}
|
||||
}
|
||||
catch (const std::exception &ex)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -33,15 +33,65 @@
|
|||
#include <nav_2d_utils/parameters.h>
|
||||
#include <boost/dll/import.hpp>
|
||||
#include <robot/console.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
#include <tf3/transform_datatypes.h>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
try {
|
||||
std::shared_ptr<tf3::BufferCore> tf = std::make_shared<tf3::BufferCore>();
|
||||
|
||||
// Lambda function để set static transform (tương tự tf_listener_set_static_transform trong nav_c_api.cpp)
|
||||
auto set_static_transform = [&tf](const std::string& parent_frame,
|
||||
const std::string& child_frame,
|
||||
double x, double y, double z,
|
||||
double qx, double qy, double qz, double qw) -> bool {
|
||||
if (!tf || parent_frame.empty() || child_frame.empty()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
try {
|
||||
tf3::TransformStampedMsg st;
|
||||
st.header.stamp = tf3::Time::now();
|
||||
st.header.frame_id = parent_frame;
|
||||
st.child_frame_id = child_frame;
|
||||
st.transform.translation.x = x;
|
||||
st.transform.translation.y = y;
|
||||
st.transform.translation.z = z;
|
||||
st.transform.rotation.x = qx;
|
||||
st.transform.rotation.y = qy;
|
||||
st.transform.rotation.z = qz;
|
||||
st.transform.rotation.w = qw;
|
||||
|
||||
return tf->setTransform(st, "move_base_main(static)", true /*is_static*/);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
if (set_static_transform("map", "odom", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
|
||||
robot::log_info("[%s:%d]\n Static transform set: map -> odom", __FILE__, __LINE__);
|
||||
} else {
|
||||
robot::log_error("[%s:%d]\n Failed to set static transform: map -> odom", __FILE__, __LINE__);
|
||||
}
|
||||
|
||||
if (set_static_transform("odom", "base_footprint", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
|
||||
robot::log_info("[%s:%d]\n Static transform set: odom -> base_footprint", __FILE__, __LINE__);
|
||||
} else {
|
||||
robot::log_error("[%s:%d]\n Failed to set static transform: odom -> base_footprint", __FILE__, __LINE__);
|
||||
}
|
||||
|
||||
if (set_static_transform("base_footprint", "base_link", 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) {
|
||||
robot::log_info("[%s:%d]\n Static transform set: base_footprint -> base_link", __FILE__, __LINE__);
|
||||
} else {
|
||||
robot::log_error("[%s:%d]\n Failed to set static transform: base_footprint -> base_link", __FILE__, __LINE__);
|
||||
}
|
||||
|
||||
robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
|
||||
robot::PluginLoaderHelper loader;
|
||||
std::string path_file_so = loader.findLibraryPath("MoveBase");
|
||||
auto create_plugin = boost::dll::import_alias<move_base_core::BaseNavigation::Ptr()>(
|
||||
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so",
|
||||
path_file_so,
|
||||
"MoveBase", boost::dll::load_mode::append_decorations);
|
||||
|
||||
robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
|
||||
|
|
@ -57,7 +107,6 @@ int main(int argc, char** argv)
|
|||
robot::log_info("[%s:%d]\n Navigation initialized successfully", __FILE__, __LINE__);
|
||||
|
||||
std::cout << "Move Base Main Function :" << robot::Time::now().toSec() << std::endl;
|
||||
|
||||
// Explicitly reset before create_plugin goes out of scope to ensure proper cleanup order
|
||||
robot::log_info("[%s:%d]\n Cleaning up...", __FILE__, __LINE__);
|
||||
nav_ptr.reset();
|
||||
|
|
@ -71,29 +120,5 @@ int main(int argc, char** argv)
|
|||
robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
// robot::NodeHandle private_nh_ = robot::NodeHandle("~");
|
||||
// robot::NodeHandle adapter_nh_ = robot::NodeHandle(private_nh_, "LocalPlannerAdapter");
|
||||
// robot::log_info_at(__FILE__, __LINE__, "\nUsing adapter namespace: %s", adapter_nh_.getNamespace().c_str());
|
||||
|
||||
|
||||
// std::string planner_name;
|
||||
// if (adapter_nh_.hasParam("planner_name"))
|
||||
// {
|
||||
// adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
|
||||
// adapter_nh_.setParam("planner_name", planner_name);
|
||||
// }
|
||||
// robot::log_info_at(__FILE__, __LINE__, "Planner name: %s", planner_name.c_str());
|
||||
// robot::NodeHandle parent_;
|
||||
// parent_ = adapter_nh_;
|
||||
// robot::NodeHandle planner_nh_ = robot::NodeHandle(parent_, "PNKXLocalPlanner");
|
||||
// std::string algorithm_nav_name;
|
||||
// planner_nh_.getParam("algorithm_nav_name", algorithm_nav_name, std::string("pnkx_local_planner::PTA"));
|
||||
// robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user