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-16 16:18:35 +07:00
parent a06beb70b8
commit 89f435633c
47 changed files with 600 additions and 881 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
CMakeLists.txt
View File

@ -15,7 +15,9 @@ if(NOT CMAKE_BUILD_TYPE)
endif()
# Flags chung
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
@ -64,10 +66,6 @@ if (NOT TARGET voxel_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/voxel_grid)
endif()
if (NOT TARGET nav_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid)
endif()
if (NOT TARGET nav_2d_msgs)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/nav_2d_msgs)
endif()
@ -84,11 +82,14 @@ if (NOT TARGET nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
endif()
if (NOT TARGET nav_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid)
endif()
if (NOT TARGET nav_core2)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
endif()
if (NOT TARGET nav_core_adapter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
endif()
@ -113,9 +114,9 @@ if (NOT TARGET score_algorithm)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Cores/score_algorithm)
endif()
#if (NOT TARGET mkt_algorithm)
# add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
#endif()
if (NOT TARGET mkt_algorithm)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Libraries/mkt_algorithm)
endif()
if (NOT TARGET two_points_planner)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/global_planners/two_points_planner)

0
config/mprim/costmap_common_params.yaml → config/costmap_common_params.yaml
View File

0
config/mprim/costmap_global_params.yaml → config/costmap_global_params.yaml
View File

0
config/mprim/costmap_global_params_plugins_no_virtual_walls.yaml → config/costmap_global_params_plugins_no_virtual_walls.yaml
View File

0
config/mprim/costmap_local_params.yaml → config/costmap_local_params.yaml
View File

0
config/mprim/move_base_common_params.yaml → config/move_base_common_params.yaml
View File

2
config/mprim/pnkx_local_planner_params.yaml → config/pnkx_local_planner_params.yaml
View File

@ -4,7 +4,7 @@ docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: nav_core_adapter::LocalPlannerAdapter
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)

0
config/mprim/two_points_global_params.yaml → config/two_points_global_params.yaml
View File

4
src/APIs/c_api/CMakeLists.txt
View File

@ -69,7 +69,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@ -25,6 +25,8 @@ add_library(score_algorithm src/score_algorithm.cpp)
target_link_libraries(score_algorithm
PUBLIC
${PACKAGES_DIR}
robot::node_handle
robot::console
PRIVATE
Boost::filesystem
Boost::system
@ -55,7 +57,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@ -1,26 +0,0 @@
#ifndef ANGLES_ANGLES_H
#define ANGLES_ANGLES_H
#include <cmath>
namespace angles
{
/**
* @brief Normalize an angle to the range [-π, π]
* @param angle The angle in radians to normalize
* @return The normalized angle in the range [-π, π]
*/
inline double normalize_angle(double angle)
{
while (angle > M_PI)
angle -= 2.0 * M_PI;
while (angle < -M_PI)
angle += 2.0 * M_PI;
return angle;
}
} // namespace angles
#endif // ANGLES_ANGLES_H

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

@ -1,7 +1,7 @@
#ifndef _SCORE_ALGORITHM_H_INCLUDED__
#define _SCORE_ALGORITHM_H_INCLUDED__
#include <yaml-cpp/yaml.h>
#include <robot/node_handle.h>
#include <iostream>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <nav_2d_msgs/Path2D.h>
@ -24,7 +24,7 @@ namespace score_algorithm
class ScoreAlgorithm
{
public:
using Ptr = boost::shared_ptr<score_algorithm::ScoreAlgorithm>;
using Ptr = std::shared_ptr<score_algorithm::ScoreAlgorithm>;
inline double smoothstep(double x)
{
@ -38,7 +38,7 @@ namespace score_algorithm
* @param tf TFListener pointer
* @param costmap_ros Costmap pointer
*/
virtual void initialize(YAML::Node &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_ros,
virtual void initialize(robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot,
const score_algorithm::TrajectoryGenerator::Ptr &traj) = 0;
/**

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

@ -1,7 +1,7 @@
#ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
#define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H
#include <yaml-cpp/yaml.h>
#include <robot/node_handle.h>
#include <mkt_msgs/Trajectory2D.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Twist2D.h>
@ -39,7 +39,7 @@ namespace score_algorithm
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const YAML::Node &nh) = 0;
virtual void initialize(const robot::NodeHandle &nh) = 0;
/**
* @brief Reset the state (if any) when the planner gets a new goal
@ -128,7 +128,8 @@ namespace score_algorithm
const nav_2d_msgs::Twist2D &start_vel,
const nav_2d_msgs::Twist2D &cmd_vel) = 0;
virtual YAML::Node getNodeHandle() = 0;
virtual robot::NodeHandle getNodeHandle() = 0;
};
} // namespace score_algorithm

22
src/Algorithms/Libraries/angles/include/angles/angles.h
View File

@ -20,6 +20,28 @@ inline double normalize_angle(double angle)
return angle;
}
/**
* @brief Calculate the shortest angular distance between two angles
*
* Computes the shortest angular distance from angle 'from' to angle 'to'.
* The result is in the range [-π, π], where positive values indicate
* counter-clockwise rotation and negative values indicate clockwise rotation.
*
* @param from The starting angle in radians
* @param to The target angle in radians
* @return The shortest angular distance in radians, in the range [-π, π]
*
* @code
* double diff = angles::shortest_angular_distance(0.0, M_PI/2); // Returns π/2
* double diff2 = angles::shortest_angular_distance(M_PI, 0.0); // Returns -π (or π, normalized)
* @endcode
*/
inline double shortest_angular_distance(double from, double to)
{
double diff = to - from;
return normalize_angle(diff);
}
} // namespace angles
#endif // ANGLES_ANGLES_H

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

@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@ -22,16 +22,15 @@ include_directories(
# Dependencies packages
set(PACKAGES_DIR
geometry_msgs
score_algorithm
nav_2d_msgs
nav_2d_utils
kalman
geometry_msgs
score_algorithm
nav_2d_msgs
nav_2d_utils
kalman
angles
nav_grid
costmap_2d
costmap_2d
sensor_msgs
visualization_msgs
std_msgs
)
@ -53,6 +52,8 @@ add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
target_link_libraries(mkt_algorithm_diff
PUBLIC
${PACKAGES_DIR}
robot::node_handle
robot::console
Boost::system
Eigen3::Eigen
)
@ -97,7 +98,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@ -24,7 +24,7 @@ namespace mkt_algorithm
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
/**
* @brief Calculating algorithm
@ -35,6 +35,12 @@ namespace mkt_algorithm
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief Create a new GoStraight instance
* @return A pointer to the new GoStraight instance
*/
static score_algorithm::ScoreAlgorithm::Ptr create();
}; // class GoStraight
} // namespace diff

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

@ -1,26 +1,18 @@
#ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__
#include <robot/node_handle.h>
#include <score_algorithm/score_algorithm.h>
#include <boost/dll/import.hpp>
#include <boost/dll/alias.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_2d_msgs/Pose2DStamped.h>
// #include <visualization_msgs/Marker.h>
// #include <ddynamic_reconfigure/ddynamic_reconfigure.h>
// #include <base_local_planner/costmap_model.h>
// #include <base_local_planner/world_model.h>
// #include <base_local_planner/trajectory_planner.h>
// #include <base_local_planner/map_grid_visualizer.h>
// #include <base_local_planner/BaseLocalPlannerConfig.h>
// #include <teb_local_planner/pose_se2.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
#include <tf3/exceptions.h>
#include <tf3/utils.h>
#include <nav_msgs/Path.h>
#include <kalman/kalman.h>
@ -52,7 +44,7 @@ namespace mkt_algorithm
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, 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
@ -92,6 +84,12 @@ namespace mkt_algorithm
*/
virtual void resume() override;
/**
* @brief Create a new PredictiveTrajectory instance
* @return A pointer to the new PredictiveTrajectory instance
*/
static score_algorithm::ScoreAlgorithm::Ptr create();
protected:
inline double sign(double x)
{
@ -107,7 +105,7 @@ namespace mkt_algorithm
* @brief Initialize dynamic reconfigure
* @param nh NodeHandle to read parameters from
*/
virtual void initDynamicReconfigure(const ros::NodeHandle &nh);
virtual void initDynamicReconfigure(const robot::NodeHandle &nh);
/**
* @brief Dynamically adjust look ahead distance based on the speed
@ -160,7 +158,7 @@ namespace mkt_algorithm
*/
bool transformGlobalPlan(
TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
nav_2d_msgs::Path2D &transformed_plan);
/**
@ -241,17 +239,15 @@ namespace mkt_algorithm
bool initialized_;
bool nav_stop_, avoid_obstacles_;
ros::NodeHandle nh_, nh_priv_;
robot::NodeHandle nh_, nh_priv_;
std::string frame_id_path_;
std::string robot_base_frame_;
ros::Publisher carrot_pub_;
nav_2d_msgs::Pose2DStamped goal_;
nav_2d_msgs::Path2D global_plan_;
nav_2d_msgs::Path2D compute_plan_;
nav_2d_msgs::Path2D transform_plan_;
nav_2d_msgs::Twist2D prevous_drive_cmd_;
ros::Publisher drive_pub_;
double x_direction_, y_direction_, theta_direction_;
double max_robot_pose_search_dist_;
@ -296,16 +292,9 @@ namespace mkt_algorithm
// Control frequency
double control_duration_;
std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddr_;
base_local_planner::BaseLocalPlannerConfig config_;
base_local_planner::WorldModel *world_model_; ///< @brief The world model that the controller will use
base_local_planner::TrajectoryPlanner *tc_; ///< @brief The trajectory controller
base_local_planner::MapGridVisualizer map_viz_;
std::vector<geometry_msgs::Point> footprint_spec_;
ros::Time last_actuator_update_;
robot::Time last_actuator_update_;
boost::shared_ptr<KalmanFilter> kf_;
int m_, n_;
Eigen::MatrixXd A;
@ -314,8 +303,6 @@ namespace mkt_algorithm
Eigen::MatrixXd R;
Eigen::MatrixXd P;
ros::Publisher cost_right_goals_pub_, cost_left_goals_pub_;
// visualization_msgs::Marker L_, R_;
}; // class PredictiveTrajectory
} // namespace diff

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

@ -23,7 +23,7 @@ namespace mkt_algorithm
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj) override;
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, 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
@ -53,6 +53,12 @@ namespace mkt_algorithm
virtual mkt_msgs::Trajectory2D calculator(
const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief Create a new RotateToGoal instance
* @return A pointer to the new RotateToGoal instance
*/
static score_algorithm::ScoreAlgorithm::Ptr create();
protected:
/**
* @brief Initialize parameters

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

@ -59,7 +59,6 @@
<build_depend>ddynamic_reconfigure</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>base_local_planner</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>score_algorithm</build_export_depend>
@ -71,7 +70,6 @@
<build_export_depend>ddynamic_reconfigure</build_export_depend>
<build_export_depend>costmap_2d</build_export_depend>
<build_export_depend>base_local_planner</build_export_depend>
<build_export_depend>visualization_msgs</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>score_algorithm</exec_depend>
@ -83,7 +81,6 @@
<exec_depend>ddynamic_reconfigure</exec_depend>
<exec_depend>costmap_2d</exec_depend>
<exec_depend>base_local_planner</exec_depend>
<exec_depend>visualization_msgs</exec_depend>
<depend>pluginlib</depend>

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

@ -1,35 +1,24 @@
#include <mkt_algorithm/diff/diff_go_straight.h>
// other
// #include <pluginlib/class_list_macros.h>
#include <boost/dll/alias.hpp>
#include <robot/console.h>
void mkt_algorithm::diff::GoStraight::initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
if (!initialized_)
{
nh_ = ros::NodeHandle("~/");
nh_priv_ = ros::NodeHandle("~/" + name);
nh_ = robot::NodeHandle("~");
nh_priv_ = robot::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_ros_ = costmap_ros;
costmap_robot_ = costmap_robot;
this->getParams();
// this->initDynamicReconfigure(nh_priv_);
nh_.param("publish_topic", enable_publish_, false);
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
std::vector<geometry_msgs::Point> footprint = costmap_ros_? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
std::vector<geometry_msgs::Point> footprint = costmap_robot_? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
@ -53,7 +42,7 @@ void mkt_algorithm::diff::GoStraight::initialize(
}
// kalman
last_actuator_update_ = ros::Time::now();
last_actuator_update_ = robot::Time::now();
n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
m_ = 2; // measurements: x, y, theta
double dt = control_duration_;
@ -87,11 +76,11 @@ void mkt_algorithm::diff::GoStraight::initialize(
kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
Eigen::VectorXd x0(n_);
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(ros::Time::now().toSec(), x0);
kf_->init(robot::Time::now().toSec(), x0);
x_direction_ = y_direction_ = theta_direction_ = 0;
initialized_ = true;
ROS_INFO("GoStraight Initialized successfully");
robot::log_info("GoStraight Initialized successfully");
}
}
@ -104,11 +93,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
return result;
if (compute_plan_.poses.size() < 2)
{
ROS_WARN_THROTTLE(2.0, "Local compute plan is available");
robot::log_warning("Local compute plan is available");
return result;
}
ros::Time current_time = ros::Time::now();
robot::Time current_time = robot::Time::now();
double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
@ -117,8 +106,8 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity); // Nhận tốc độ hiện tại từ odom
ros::Rate r(50);
while (ros::ok() && traj_->hasMoreTwists())
robot::Rate r(50);
while (traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
// ROS_INFO("Twist: x: %.2f, y: %.2f, theta: %.2f", twist.x, twist.y, twist.theta);
@ -129,13 +118,10 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
nav_2d_msgs::Path2D transformed_plan = transform_plan_;
if (transformed_plan.poses.empty())
{
ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
robot::log_warning("Transformed plan is empty. Cannot determine a local plan.");
return result;
}
if (enable_publish_)
transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
// Dynamically adjust look ahead distance based on the speed
double lookahead_dist = this->getLookAheadDistance(twist);
// Return false if the transformed global plan is empty
@ -145,9 +131,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
// Get lookahead point and publish for visualization
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
if (enable_publish_)
carrot_pub_.publish(this->createCarrotMsg(carrot_pose));
// Carrot distance squared
const double carrot_dist2 =
(carrot_pose.pose.x * carrot_pose.pose.x) +
@ -240,4 +223,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::GoStraight::calculator(
return result;
}
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::GoStraight, score_algorithm::ScoreAlgorithm)
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::GoStraight::create()
{
return std::make_shared<mkt_algorithm::diff::GoStraight>();
}
BOOST_DLL_ALIAS(mkt_algorithm::diff::GoStraight::create, MKTAlgorithmDiffGoStraight)

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

@ -1,93 +1,27 @@
#include <mkt_algorithm/diff/diff_predictive_trajectory.h>
// other
#include <pluginlib/class_list_macros.h>
#include <boost/dll/import.hpp>
#include <robot/console.h>
#define LIMIT_VEL_THETA 0.33
void mkt_algorithm::diff::PredictiveTrajectory::initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
if (!initialized_)
{
nh_ = ros::NodeHandle("~/");
nh_priv_ = ros::NodeHandle("~/" + name);
nh_ = robot::NodeHandle("~");
nh_priv_ = robot::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_ros_ = costmap_ros;
costmap_robot_ = costmap_robot;
this->getParams();
// this->initDynamicReconfigure(nh_priv_);
nh_.param("publish_topic", enable_publish_, false);
nh_.param<double>("min_approach_linear_velocity", min_approach_linear_velocity_, 0.1);
std::vector<double> y_vels = {};
footprint_spec_ = costmap_ros_->getRobotFootprint();
world_model_ = new base_local_planner::CostmapModel(*costmap_ros->getCostmap());
this->config_.acc_lim_x = fabs(acc_lim_x_);
this->config_.acc_lim_y = fabs(acc_lim_y_);
this->config_.acc_lim_theta = fabs(acc_lim_theta_);
this->config_.max_vel_x = max_vel_x_;
this->config_.min_vel_x = fabs(min_vel_x_);
this->config_.max_vel_theta = max_vel_theta_;
this->config_.min_vel_theta = (-1.0) * max_vel_theta_;
this->config_.min_in_place_vel_theta = 0.4;
this->config_.sim_time = 1.5;
this->config_.sim_granularity = 0.025;
this->config_.vx_samples = 10;
this->config_.vtheta_samples = 40;
this->config_.path_distance_bias = 0.6;
this->config_.goal_distance_bias = 0.4;
this->config_.occdist_scale = 0.2;
this->config_.heading_lookahead = 1.0;
this->config_.oscillation_reset_dist = 0.05;
this->config_.escape_reset_dist = 0.1;
this->config_.escape_reset_theta = 1.570796;
this->config_.escape_vel = -0.1;
this->config_.holonomic_robot = false;
this->config_.heading_scoring_timestep = 0.1;
this->config_.dwa = true;
this->config_.simple_attractor = true;
this->config_.heading_scoring = false;
this->config_.angular_sim_granularity = 0.025;
bool meter_scoring = false;
double stop_time_buffer = 0.2;
tc_ = new base_local_planner::TrajectoryPlanner(
*world_model_, *costmap_ros->getCostmap(), footprint_spec_,
this->config_.acc_lim_x, this->config_.acc_lim_y, this->config_.acc_lim_theta,
this->config_.sim_time, this->config_.sim_granularity,
this->config_.vx_samples, this->config_.vtheta_samples,
this->config_.path_distance_bias, this->config_.goal_distance_bias, this->config_.occdist_scale,
this->config_.heading_lookahead, this->config_.oscillation_reset_dist,
this->config_.escape_reset_dist, this->config_.escape_reset_theta,
this->config_.holonomic_robot,
this->config_.max_vel_x, this->config_.min_vel_x,
this->config_.max_vel_theta, this->config_.min_vel_theta, this->config_.min_in_place_vel_theta,
this->config_.escape_vel,
this->config_.dwa, this->config_.heading_scoring, this->config_.heading_scoring_timestep, meter_scoring, this->config_.simple_attractor,
y_vels, stop_time_buffer, control_duration_, this->config_.angular_sim_granularity);
map_viz_.initialize(name,
costmap_ros_->getGlobalFrameID(),
[this](int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)
{
return tc_->getCellCosts(cx, cy, path_cost, goal_cost, occ_cost, total_cost);
});
carrot_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("lookahead_point", 1);
reached_intermediate_goals_pub_ = nh_.advertise<geometry_msgs::PoseArray>("reached_intermediate_goals", 1);
current_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("sub_goal", 1);
closet_robot_goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("closet_robot_goal", 1);
transformed_plan_pub_ = nh_.advertise<nav_msgs::Path>("transformed_plan", 1);
compute_plan_pub_ = nh_.advertise<nav_msgs::Path>("compute_plan", 1);
cost_right_goals_pub_ = nh_.advertise<visualization_msgs::Marker>("cost_right_goals", 1);
cost_left_goals_pub_ = nh_.advertise<visualization_msgs::Marker>("cost_left_goals", 1);
drive_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_raw", 1);
std::vector<geometry_msgs::Point> footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
footprint_spec_ = costmap_robot_->getRobotFootprint();
std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
@ -111,7 +45,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
}
// kalman
last_actuator_update_ = ros::Time::now();
last_actuator_update_ = robot::Time::now();
n_ = 6; // [x, vx, ax, y, vy, ay, theta, vtheta, atheta]
m_ = 2; // measurements: x, y, theta
double dt = control_duration_;
@ -145,47 +79,20 @@ void mkt_algorithm::diff::PredictiveTrajectory::initialize(
kf_ = boost::make_shared<KalmanFilter>(dt, A, C, Q, R, P);
Eigen::VectorXd x0(n_);
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(ros::Time::now().toSec(), x0);
L_.header.frame_id = R_.header.frame_id = costmap_ros_->getGlobalFrameID(); // hoặc frame phù hợp
L_.ns = "left_cost_points";
R_.ns = "right_cost_points";
L_.id = 0;
R_.id = 1;
L_.type = R_.type = visualization_msgs::Marker::POINTS;
L_.action = R_.action = visualization_msgs::Marker::ADD;
L_.scale.x = L_.scale.y = R_.scale.x = R_.scale.y = 0.05; // kích thước điểm
// màu: L = xanh lá, R = đỏ
L_.color.r = 0.0;
L_.color.g = 1.0;
L_.color.b = 0.0;
L_.color.a = 1.0;
R_.color.r = 1.0;
R_.color.g = 0.0;
R_.color.b = 0.0;
R_.color.a = 1.0;
kf_->init(robot::Time::now().toSec(), x0);
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
ROS_INFO("PredictiveTrajectory Initialized successfully");
robot::log_info("[%s:%d] PredictiveTrajectory Initialized successfully", __FILE__, __LINE__);
}
}
mkt_algorithm::diff::PredictiveTrajectory::~PredictiveTrajectory()
{
if (ddr_)
ddr_.reset();
if (tc_ != NULL)
delete tc_;
if (world_model_ != NULL)
delete world_model_;
}
void mkt_algorithm::diff::PredictiveTrajectory::getParams()
{
robot_base_frame_ = nav_2d_utils::searchAndGetParam(nh_priv_, "robot_base_frame", std::string("base_link"));
robot_base_frame_ = nh_priv_.param<std::string>("robot_base_frame", std::string("base_link"));
nh_priv_.param<bool>("avoid_obstacles", avoid_obstacles_, false);
nh_priv_.param<double>("xy_local_goal_tolerance", xy_local_goal_tolerance_, 0.5);
nh_priv_.param<double>("angle_threshold", angle_threshold_, M_PI / 8);
@ -220,7 +127,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
if (inflation_cost_scaling_factor_ <= 0.0)
{
ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, "
"it should be >0. Disabling cost regulated linear velocity scaling.");
use_cost_regulated_linear_velocity_scaling_ = false;
}
@ -247,59 +154,25 @@ void mkt_algorithm::diff::PredictiveTrajectory::getParams()
}
}
void mkt_algorithm::diff::PredictiveTrajectory::initDynamicReconfigure(const ros::NodeHandle &nh)
{
// Ddynamic Reconfigure
ddr_ = std::make_shared<ddynamic_reconfigure::DDynamicReconfigure>(nh);
ddr_->registerVariable<bool>("avoid_obstacles", &this->avoid_obstacles_, "", true);
ddr_->registerVariable<double>("lookahead_time", &this->lookahead_time_, "", 0.0, 20.0);
ddr_->registerVariable<double>("lookahead_dist", &this->lookahead_dist_, "", 0.0, 20.0);
ddr_->registerVariable<bool>("use_velocity_scaled_lookahead_dist", &this->use_velocity_scaled_lookahead_dist_);
ddr_->registerVariable<double>("min_lookahead_dist", &this->min_lookahead_dist_, "", 0.0, 5.0);
ddr_->registerVariable<double>("max_lookahead_dist", &this->max_lookahead_dist_, "", 0.0, 10.0);
ddr_->registerVariable<double>("min_journey_squared", &this->min_journey_squared_, "", 0.0, 1.0);
ddr_->registerVariable<double>("max_journey_squared", &this->max_journey_squared_, "", 0.0, 1.0);
// Rotate to heading param
ddr_->registerVariable<bool>("use_rotate_to_heading", &this->use_rotate_to_heading_);
ddr_->registerVariable<double>("rotate_to_heading_min_angle", &this->rotate_to_heading_min_angle_, "", 0.0, 15.0);
// Speed
ddr_->registerVariable<double>("min_approach_linear_velocity", &this->min_approach_linear_velocity_, "", 0.0, 10.0);
// Regulated linear velocity scaling
ddr_->registerVariable<bool>("use_regulated_linear_velocity_scaling", &this->use_regulated_linear_velocity_scaling_);
ddr_->registerVariable<double>("regulated_linear_scaling_min_radius", &this->regulated_linear_scaling_min_radius_, "", 0.0, 5.0);
ddr_->registerVariable<double>("regulated_linear_scaling_min_speed", &this->regulated_linear_scaling_min_speed_, "", 0.0, 5.0);
// Inflation cost scaling (Limit velocity by proximity to obstacles)
ddr_->registerVariable<bool>("use_cost_regulated_linear_velocity_scaling", &this->use_cost_regulated_linear_velocity_scaling_);
ddr_->registerVariable<double>("inflation_cost_scaling_factor", &this->inflation_cost_scaling_factor_, "", 0.0, 10.0);
ddr_->registerVariable<double>("cost_scaling_dist", &this->cost_scaling_dist_, "", 0.0, 10.0);
ddr_->registerVariable<double>("cost_scaling_gain", &this->cost_scaling_gain_, "", 0.0, 10.0);
ddr_->publishServicesTopics();
}
void mkt_algorithm::diff::PredictiveTrajectory::reset()
{
if (this->initialized_)
{
ROS_INFO("PredictiveTrajectory is reset");
robot::log_info("[%s:%d] PredictiveTrajectory is reset", __FILE__, __LINE__);
reached_intermediate_goals_.clear();
start_index_saved_vt_.clear();
this->clear();
x_direction_ = y_direction_ = theta_direction_ = 0;
this->follow_step_path_ = false;
this->nav_stop_ = false;
last_actuator_update_ = ros::Time::now();
last_actuator_update_ = robot::Time::now();
prevous_drive_cmd_ = nav_2d_msgs::Twist2D();
if (kf_)
{
Eigen::VectorXd x0(n_);
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(ros::Time::now().toSec(), x0);
kf_->init(robot::Time::now().toSec(), x0);
}
}
}
@ -316,13 +189,13 @@ void mkt_algorithm::diff::PredictiveTrajectory::resume()
if(!this->nav_stop_)
return;
prevous_drive_cmd_ = nav_2d_msgs::Twist2D();
last_actuator_update_ = ros::Time::now();
last_actuator_update_ = robot::Time::now();
if (kf_)
{
Eigen::VectorXd x0(n_);
x0 << 0, 0, 0, 0, 0, 0;
kf_->init(ros::Time::now().toSec(), x0);
kf_->init(robot::Time::now().toSec(), x0);
}
this->nav_stop_ = false;
}
@ -334,11 +207,11 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
{
if (!initialized_)
{
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
robot::log_error("[%s:%d] This planner has not been initialized, please call initialize() before using this planner", __FILE__, __LINE__);
return false;
}
std::vector<geometry_msgs::Point> footprint = costmap_ros_ ? costmap_ros_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
std::vector<geometry_msgs::Point> footprint = costmap_robot_ ? costmap_robot_->getRobotFootprint() : std::vector<geometry_msgs::Point>();
if (footprint.size() > 1)
{
double min_length = 1e10;
@ -363,12 +236,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
if (global_plan.poses.empty() || (unsigned int)global_plan.poses.size() < 2)
{
ROS_ERROR("The Local plan is empty or less than 1 points %d", (unsigned int)global_plan.poses.size());
robot::log_error("[%s:%d] The Local plan is empty or less than 1 points %d", __FILE__, __LINE__, (unsigned int)global_plan.poses.size());
return false;
}
this->getParams();
if (avoid_obstacles_)
map_viz_.publishCostCloud(costmap_ros_->getCostmap());
frame_id_path_ = global_plan.header.frame_id;
goal_ = goal;
@ -377,13 +248,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
// prune global plan to cut off parts of the past (spatially before the robot)
if (!this->pruneGlobalPlan(tf_, pose, global_plan_, 1.0))
{
ROS_ERROR("pruneGlobalPlan Failed");
robot::log_error("[%s:%d] pruneGlobalPlan Failed", __FILE__, __LINE__);
return false;
}
double S = std::numeric_limits<double>::infinity();
S = std::max(costmap_ros_->getCostmap()->getSizeInCellsX() * costmap_ros_->getCostmap()->getResolution() / 2.0,
costmap_ros_->getCostmap()->getSizeInCellsY() * costmap_ros_->getCostmap()->getResolution() / 2.0);
S = std::max(costmap_robot_->getCostmap()->getSizeInCellsX() * costmap_robot_->getCostmap()->getResolution() / 2.0,
costmap_robot_->getCostmap()->getSizeInCellsY() * costmap_robot_->getCostmap()->getResolution() / 2.0);
const double min_S = min_lookahead_dist_ * 1.5 + max_path_distance_, max_S = max_lookahead_dist_ * 1.5 + max_path_distance_;
S = std::clamp(S * fabs(velocity.x) * lookahead_time_, min_S, max_S);
compute_plan_.poses.clear();
@ -392,13 +263,13 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
{
double dx = global_plan_.poses.back().pose.x - global_plan_.poses.front().pose.x;
double dy = global_plan_.poses.back().pose.y - global_plan_.poses.front().pose.y;
if (hypot(dx, dy) <= 2.0 * costmap_ros_->getCostmap()->getResolution())
if (hypot(dx, dy) <= 2.0 * costmap_robot_->getCostmap()->getResolution())
{
compute_plan_ = global_plan_;
}
else
{
ROS_ERROR("The Local Plan has 2 points and hypot between more than is %.3f m", costmap_ros_->getCostmap()->getResolution());
robot::log_error("[%s:%d] The Local Plan has 2 points and hypot between more than is %.3f m", __FILE__, __LINE__, costmap_robot_->getCostmap()->getResolution());
return false;
}
}
@ -407,24 +278,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
unsigned int start_index, goal_index;
if (!this->computePlanCommand(pose, velocity, S, global_plan_, start_index, goal_index, compute_plan_))
{
ROS_ERROR("computePlanCommand is Failed");
robot::log_error("[%s:%d] computePlanCommand is Failed", __FILE__, __LINE__);
return false;
}
}
if (enable_publish_)
compute_plan_pub_.publish(nav_2d_utils::pathToPath(compute_plan_));
double lookahead_dist = this->getLookAheadDistance(velocity);
transform_plan_.poses.clear();
if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_ros_, robot_base_frame_, lookahead_dist, transform_plan_))
if (!this->transformGlobalPlan(tf_, compute_plan_, pose, costmap_robot_, robot_base_frame_, lookahead_dist, transform_plan_))
{
ROS_WARN("Could not transform the global plan to the frame of the controller");
robot::log_warning("[%s:%d] Could not transform the global plan to the frame of the controller", __FILE__, __LINE__);
return false;
}
// else
// {
// ROS_INFO_THROTTLE(0.5, "Transform plan journey: %f %f %f", journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
// robot::log_info("[%s:%d] Transform plan journey: %f %f %f", __FILE__, __LINE__, journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1), min_lookahead_dist_, max_lookahead_dist_);
// }
x_direction = x_direction_;
@ -462,29 +330,30 @@ bool mkt_algorithm::diff::PredictiveTrajectory::prepare(const nav_2d_msgs::Pose2
double dx = it->pose.x - carrot_pose_it->pose.x;
double dy = it->pose.x - carrot_pose_it->pose.y;
distance_it += std::hypot(dx, dy);
if (distance_it > costmap_ros_->getCostmap()->getResolution())
if (distance_it > costmap_robot_->getCostmap()->getResolution())
{
prev_carrot_pose_it = it;
break;
}
}
geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_ros_->getCostmap()->getResolution()
geometry_msgs::Pose front = journey(transform_plan_.poses, 0, transform_plan_.poses.size() - 1) > 5.0 * costmap_robot_->getCostmap()->getResolution()
? nav_2d_utils::pose2DToPose((*(prev_carrot_pose_it)).pose)
: nav_2d_utils::pose2DToPose(geometry_msgs::Pose2D());
geometry_msgs::Pose back = nav_2d_utils::pose2DToPose((*(carrot_pose_it)).pose);
teb_local_planner::PoseSE2 start_pose(front);
teb_local_planner::PoseSE2 goal_pose(back);
const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
// teb_local_planner::PoseSE2 start_pose(front);
// teb_local_planner::PoseSE2 goal_pose(back);
// const double dir_path = (goal_pose.position() - start_pose.position()).dot(start_pose.orientationUnitVec());
const double dir_path = 0.0;
if (fabs(dir_path) > M_PI / 6 || x_direction < 1e-9)
x_direction = dir_path > 0 ? FORWARD : BACKWARD;
}
catch (std::exception &e)
{
ROS_ERROR("getLookAheadPoint throw an exception: %s", e.what());
robot::log_error("[%s:%d] getLookAheadPoint throw an exception: %s", __FILE__, __LINE__, e.what());
return false;
}
}
@ -504,11 +373,11 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
return result;
if (compute_plan_.poses.size() < 2)
{
ROS_WARN("Local compute plan is available");
robot::log_warning("[%s:%d] Local compute plan is available", __FILE__, __LINE__);
return result;
}
ros::Time current_time = ros::Time::now();
robot::Time current_time = robot::Time::now();
double dt = (current_time - last_actuator_update_).toSec();
last_actuator_update_ = current_time;
@ -516,77 +385,29 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
double sign_x = sign(x_direction_);
nav_2d_msgs::Twist2D twist;
traj_->startNewIteration(velocity);
while (ros::ok() && traj_->hasMoreTwists())
while (traj_->hasMoreTwists())
{
twist = traj_->nextTwist();
}
drive_cmd.x = std::min(sqrt(twist.x * twist.x), 1.5);
double v_max = sign_x > 0 ? traj_->getTwistLinear(true).x : traj_->getTwistLinear(false).x;
if (avoid_obstacles_)
{
if ((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))
{
nav_2d_msgs::Twist2D cmd;
cmd.x = 0.2;
moveWithAccLimits(velocity, cmd, drive_cmd);
config_.max_vel_x = drive_cmd.x;
config_.sim_time = 1.9;
config_.path_distance_bias = 0.7;
config_.goal_distance_bias = 0.6;
config_.occdist_scale = 0.05;
config_.dwa = true;
}
else if (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)
{
nav_2d_msgs::Twist2D cmd;
cmd.x = 0.3;
moveWithAccLimits(velocity, cmd, drive_cmd);
config_.max_vel_x = drive_cmd.x;
config_.sim_time = 1.9;
config_.path_distance_bias = 0.7;
config_.goal_distance_bias = 0.6;
config_.occdist_scale = 0.12;
config_.dwa = false;
}
else
{
config_.max_vel_x = std::max(drive_cmd.x, 0.2);
config_.sim_time = 1.5;
config_.path_distance_bias = 0.8;
config_.goal_distance_bias = 0.6;
config_.occdist_scale = 0.01;
config_.dwa = false;
}
tc_->reconfigure(config_);
this->publishMarkers(pose);
}
nav_2d_msgs::Path2D transformed_plan = this->transform_plan_;
if (transformed_plan.poses.empty())
{
ROS_WARN("Transformed plan is empty. Cannot determine a localglobal_plan.");
robot::log_warning("[%s:%d] Transformed plan is empty. Cannot determine a localglobal_plan.", __FILE__, __LINE__);
return result;
}
if (enable_publish_)
transformed_plan_pub_.publish(nav_2d_utils::pathToPath(transformed_plan));
double lookahead_dist = getLookAheadDistance(velocity);
double tolerance = hypot(pose.pose.x - compute_plan_.poses.front().pose.x, pose.pose.y - compute_plan_.poses.front().pose.y);
// lookahead_dist = std::clamp(lookahead_dist - tolerance * lookahead_time_, min_lookahead_dist_, max_lookahead_dist_);
if (transformed_plan.poses.empty())
{
ROS_WARN("Transformed plan is empty after compute lookahead point");
robot::log_warning("[%s:%d] Transformed plan is empty after compute lookahead point", __FILE__, __LINE__);
return result;
}
auto carrot_pose = *getLookAheadPoint(velocity, lookahead_dist, transformed_plan);
if (enable_publish_)
carrot_pub_.publish(nav_2d_utils::pose2DToPoseStamped(carrot_pose.pose, carrot_pose.header.frame_id, ros::Time::now()));
bool allow_rotate = false;
nh_priv_.param("allow_rotate", allow_rotate, false);
@ -598,7 +419,7 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
if (avoid_obstacles_)
allow_rotate = journey_plan >= distance_allow_rotate &&
fabs(front.y) <= 0.2 &&
(path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_ros_->getCostmap()->getResolution());
(path_distance_to_rotate > max_path_distance_ || path_distance_to_rotate < 2.0 * costmap_robot_->getCostmap()->getResolution());
else
allow_rotate |= path_distance_to_rotate >= distance_allow_rotate;
@ -607,7 +428,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
{
if (!stopped(velocity, max_vel_theta_ + rot_stopped_velocity_, trans_stopped_velocity_))
{
tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true);
if (!stopWithAccLimits(pose, velocity, drive_cmd))
return result;
}
@ -618,78 +438,75 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
}
else
{
bool narrow_road = (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE);
if (!avoid_obstacles_ || sign_x < 0 || journey_plan < max_journey_squared_ || narrow_road)
const double vel_x_reduce = std::min(fabs(v_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
{
const double vel_x_reduce = std::min(fabs(v_max), 0.3);
double carrot_dist2 = carrot_pose.pose.x * carrot_pose.pose.x + carrot_pose.pose.y * carrot_pose.pose.y;
carrot_dist2 = std::max(carrot_dist2, 0.05);
double curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
}
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
nav_2d_msgs::Twist2D cmd_vel, cmd_result;
cmd_vel.x = sign_x > 0
? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
: std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
drive_cmd.x = std::copysign(cmd_result.x, sign_x);
v_theta = drive_cmd.x * curvature;
}
const auto &plan_back_pose = compute_plan_.poses.back();
double post_cost = costAtPose(plan_back_pose.pose.x, plan_back_pose.pose.y);
post_cost = std::max(post_cost, center_cost_);
this->applyConstraints(0.0, lookahead_dist, curvature, twist, post_cost, drive_cmd.x, sign_x);
const double scale = fabs(velocity.x) * lookahead_time_ * 0.9;
const double min_S = min_lookahead_dist_ + max_path_distance_ + scale, max_S = max_lookahead_dist_ + max_path_distance_ + scale;
double d_reduce = std::clamp(journey_plan, min_S, max_S);
double d_begin_reduce = std::clamp(d_reduce * 0.2, 0.4, 1.0);
double cosine_factor_begin_reduce = 0.5 * (1.0 + cos(M_PI * (1.0 - fabs(journey_plan) / d_begin_reduce)));
double v_min =
journey_plan > d_begin_reduce ? vel_x_reduce : (vel_x_reduce - min_approach_linear_velocity_) * cosine_factor_begin_reduce + min_approach_linear_velocity_;
v_min *= sign_x;
double effective_journey = getEffectiveDistance(carrot_pose, journey_plan);
double decel_factor = computeDecelerationFactor(effective_journey, d_reduce);
double vel_reduce = sign_x > 0
? std::min(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min)
: std::max(drive_cmd.x, (drive_cmd.x - v_min) * decel_factor + v_min);
drive_cmd.x = (journey_plan + max_path_distance_) >= d_reduce ? drive_cmd.x : vel_reduce;
double v_theta = drive_cmd.x * curvature;
double carrot_angle = std::atan2(carrot_pose.pose.y, carrot_pose.pose.x);
if (detectWobbleByCarrotAngle(angle_history_, carrot_angle, 0.3, window_size_))
if (journey_plan < min_journey_squared_)
{
if (transform_plan_.poses.size() > 2)
{
carrot_dist2 *= 0.6;
curvature = carrot_dist2 > 0.1 ? 2.0 * carrot_pose.pose.y / carrot_dist2 : 2.0 * carrot_pose.pose.y / 0.1;
v_theta = drive_cmd.x * curvature;
nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
v_theta = atan2(dy, dx);
if (v_theta > M_PI_2)
v_theta -= M_PI;
else if (v_theta < -M_PI_2)
v_theta += M_PI;
// v_theta *= 0.5;
v_theta = std::clamp(v_theta, -0.02, 0.02);
}
if (fabs(v_theta) > LIMIT_VEL_THETA)
{
nav_2d_msgs::Twist2D cmd_vel, cmd_result;
cmd_vel.x = sign_x > 0
? std::min(drive_cmd.x, v_theta / std::max(curvature, 0.1))
: std::max(drive_cmd.x, v_theta / std::min(curvature, -0.1));
cmd_vel.x = std::clamp(cmd_vel.x, -0.5, 0.5);
this->moveWithAccLimits(velocity, cmd_vel, cmd_result);
drive_cmd.x = std::copysign(cmd_result.x, sign_x);
v_theta = drive_cmd.x * curvature;
}
if (journey_plan < min_journey_squared_)
{
if (transform_plan_.poses.size() > 2)
{
nav_2d_msgs::Pose2DStamped end = transform_plan_.poses.back();
nav_2d_msgs::Pose2DStamped start = transform_plan_.poses[transform_plan_.poses.size() - 2];
double dx = end.pose.x - start.pose.x;
double dy = end.pose.y - start.pose.y;
v_theta = atan2(dy, dx);
if (v_theta > M_PI_2)
v_theta -= M_PI;
else if (v_theta < -M_PI_2)
v_theta += M_PI;
// v_theta *= 0.5;
v_theta = std::clamp(v_theta, -0.02, 0.02);
}
else
v_theta = 0.0;
}
double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
else
v_theta = 0.0;
}
double limit_acc_theta = fabs(v_theta) > 0.15 ? acc_lim_theta_ : 1.8;
double max_acc_vth = velocity.theta + fabs(limit_acc_theta) * dt;
double min_acc_vth = velocity.theta - fabs(limit_acc_theta) * dt;
drive_cmd.theta = std::clamp(v_theta, min_acc_vth, max_acc_vth);
if (this->nav_stop_)
{
@ -703,77 +520,6 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::PredictiveTrajectory::calculator(
result.velocity = drive_cmd;
return result;
}
}
else
{
geometry_msgs::PoseStamped traj_cmd, robot_vel;
robot_vel.pose.position.x = velocity.x;
robot_vel.pose.position.y = velocity.y;
tf2::Quaternion q;
q.setRPY(0, 0, velocity.theta);
tf2::convert(q, robot_vel.pose.orientation);
tc_->updatePlan(nav_2d_utils::pathToPath(compute_plan_).poses, true);
auto path = tc_->findBestPath(nav_2d_utils::pose2DToPoseStamped(pose), robot_vel, traj_cmd);
if (path.cost_ < 0)
{
ROS_DEBUG_NAMED("trajectory_planner_ros",
"The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
throw nav_core2::PlannerTFException("The rollout planner failed to find a valid plan. This means that the footprint of the robot was in collision for all simulated trajectories.");
}
if (path.cost_ < 0 || nav_stop_)
{
if (!stopped(velocity, rot_stopped_velocity_, trans_stopped_velocity_))
{
if (!stopWithAccLimits(pose, velocity, drive_cmd))
return result;
}
else
drive_cmd = {};
result.velocity = drive_cmd;
return result;
}
else
{
drive_cmd.x = std::max(drive_cmd.x, min_approach_linear_velocity_);
drive_cmd.x = std::clamp(traj_cmd.pose.position.x, -fabs(drive_cmd.x), fabs(drive_cmd.x));
drive_cmd.y = traj_cmd.pose.position.y;
drive_cmd.theta = tf2::getYaw(traj_cmd.pose.orientation);
if ((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) &&
fabs(traj_cmd.pose.position.x) < 0.02 && fabs(drive_cmd.theta) > 0.1)
{
nav_2d_msgs::Twist2D cmd, cmd_result;
cmd.x = sign_x * min_approach_linear_velocity_;
moveWithAccLimits(velocity, cmd, cmd_result);
if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE && cost_right_goal_ >= costmap_2d::UNPREFERRED_SPACE)
drive_cmd.theta = 0.0;
else if (cost_left_goal_ >= costmap_2d::UNPREFERRED_SPACE)
drive_cmd.theta = -min_vel_theta_ * sign_x;
else
drive_cmd.theta = min_vel_theta_ * sign_x;
drive_cmd.x = sign_x * cmd_result.x;
}
for (unsigned int i = 0; i < path.getPointsSize(); ++i)
{
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
geometry_msgs::Pose2D pose;
pose.x = p_x;
pose.y = p_y;
pose.theta = p_th;
result.poses.push_back(pose);
}
}
}
Eigen::VectorXd y(2);
y << drive_cmd.x, drive_cmd.theta;
@ -1063,9 +809,9 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
if (erase_end != global_plan.poses.begin())
global_plan.poses.erase(global_plan.poses.begin(), erase_end);
}
catch (const tf::TransformException &ex)
catch (const tf3::TransformException &ex)
{
ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what());
robot::log_debug("[%s:%d] Cannot prune path since no transform is available: %s", __FILE__, __LINE__, ex.what());
return false;
}
return true;
@ -1073,7 +819,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
TFListenerPtr tf, const nav_2d_msgs::Path2D &global_plan, const nav_2d_msgs::Pose2DStamped &pose,
const costmap_2d::Costmap2DROS *costmap, const std::string &robot_base_frame, double max_plan_length,
const costmap_2d::Costmap2DROBOT *costmap, const std::string &robot_base_frame, double max_plan_length,
nav_2d_msgs::Path2D &transformed_plan)
{
// this method is a slightly modified version of base_local_planner/goal_functions.h
@ -1094,7 +840,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
{
if (global_plan.poses.empty())
{
ROS_ERROR("Received plan with zero length");
robot::log_error("[%s:%d] Received plan with zero length", __FILE__, __LINE__);
return false;
}
@ -1179,21 +925,21 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
++i;
}
}
catch (tf::LookupException &ex)
catch (tf3::LookupException &ex)
{
ROS_ERROR("No Transform available Error: %s\n", ex.what());
robot::log_error("[%s:%d] No Transform available Error: %s", __FILE__, __LINE__, ex.what());
return false;
}
catch (tf::ConnectivityException &ex)
catch (tf3::ConnectivityException &ex)
{
ROS_ERROR("Connectivity Error: %s\n", ex.what());
robot::log_error("[%s:%d] Connectivity Error: %s", __FILE__, __LINE__, ex.what());
return false;
}
catch (tf::ExtrapolationException &ex)
catch (tf3::ExtrapolationException &ex)
{
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
robot::log_error("[%s:%d] Extrapolation Error: %s", __FILE__, __LINE__, ex.what());
if (global_plan.poses.size() > 0)
ROS_ERROR("Robot Frame: %s Plan Frame size %d: %s\n", robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
robot::log_error("[%s:%d] Robot Frame: %s Plan Frame size %d: %s", __FILE__, __LINE__, robot_base_frame.c_str(), (unsigned int)global_plan.poses.size(), global_plan.header.frame_id.c_str());
return false;
}
@ -1241,29 +987,6 @@ bool mkt_algorithm::diff::PredictiveTrajectory::stopWithAccLimits(const nav_2d_m
double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - fabs(decel_lim_theta_) * dt));
// we do want to check whether or not the command is valid
if (avoid_obstacles_)
{
try
{
if (tc_ != NULL)
{
bool valid_cmd = tc_->checkTrajectory(pose.pose.x, pose.pose.y, pose.pose.theta, velocity.x, velocity.y, vel_yaw, vx, vy, vth);
// if we have a valid command, we'll pass it on, otherwise we'll command all zeros
if (valid_cmd)
{
ROS_DEBUG("Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
cmd_vel.x = vx;
cmd_vel.y = vy;
cmd_vel.theta = vth;
return true;
}
}
}
catch (const std::exception &e)
{
ROS_ERROR_STREAM(e.what() << '\n');
}
}
cmd_vel.x = vx;
cmd_vel.y = vy;
cmd_vel.theta = vth;
@ -1302,7 +1025,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
pose_cost != static_cast<double>(costmap_2d::NO_INFORMATION) &&
pose_cost != static_cast<double>(costmap_2d::FREE_SPACE))
{
const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
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)) +
inscribed_radius;
@ -1327,8 +1050,8 @@ void mkt_algorithm::diff::PredictiveTrajectory::applyConstraints(
// (2) using t with the actual lookahead
// distance to scale the velocity (e.g. t = lookahead / velocity, v = carrot / t).
double dist_error_limit = costmap_ros_ != nullptr && costmap_ros_->getCostmap() != nullptr
? 2.0 * costmap_ros_->getCostmap()->getResolution()
double dist_error_limit = costmap_robot_ != nullptr && costmap_robot_->getCostmap() != nullptr
? 2.0 * costmap_robot_->getCostmap()->getResolution()
: 0.1;
if (dist_error > dist_error_limit)
{
@ -1414,9 +1137,9 @@ double mkt_algorithm::diff::PredictiveTrajectory::costAtPose(const double &x, co
{
unsigned int mx, my;
unsigned char cost;
if (costmap_ros_->getCostmap()->worldToMap(x, y, mx, my))
if (costmap_robot_->getCostmap()->worldToMap(x, y, mx, my))
{
cost = costmap_ros_->getCostmap()->getCost(mx, my);
cost = costmap_robot_->getCostmap()->getCost(mx, my);
}
return static_cast<double>(cost);
}
@ -1490,10 +1213,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::detectWobbleByCarrotAngle(std::v
void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose2DStamped pose)
{
const auto &plan_back_pose = compute_plan_.poses.back();
// const double offset_max = this->min_path_distance_ + costmap_ros_->getCostmap()->getResolution() * 2.0;
// const double offset_max = this->min_path_distance_ + costmap_robot_->getCostmap()->getResolution() * 2.0;
// const double offset_min = this->min_path_distance_;
auto points_rb = interpolateFootprint(pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
auto points_rb = interpolateFootprint(pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points_rb)
{
double cost_goal = costAtPose(point.x, point.y);
@ -1506,19 +1229,17 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
if (y_rel > epsilon)
{
cost_left_side_ = std::max(cost_left_side_, cost_goal);
L_.points.push_back(point);
}
else if (y_rel < -epsilon)
{
cost_right_side_ = std::max(cost_right_side_, cost_goal);
R_.points.push_back(point);
}
}
unsigned int step_footprint = 10;
if ((unsigned int)(compute_plan_.poses.size() - 1) < 10)
{
auto points = interpolateFootprint(plan_back_pose.pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
auto points = interpolateFootprint(plan_back_pose.pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
@ -1531,12 +1252,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
L_.points.push_back(point);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
R_.points.push_back(point);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
@ -1546,7 +1265,7 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
{
for (unsigned int i = (unsigned int)(compute_plan_.poses.size() - 1); i > step_footprint; i -= step_footprint)
{
auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_ros_->getRobotFootprint(), costmap_ros_->getCostmap()->getResolution() * 2.0);
auto points = interpolateFootprint(compute_plan_.poses[i].pose, costmap_robot_->getRobotFootprint(), costmap_robot_->getCostmap()->getResolution() * 2.0);
for (const auto &point : points)
{
double cost_goal = costAtPose(point.x, point.y);
@ -1559,12 +1278,10 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
if (y_rel > epsilon)
{
cost_left_goal_ = std::max(cost_left_goal_, cost_goal);
L_.points.push_back(point);
}
else if (y_rel < -epsilon)
{
cost_right_goal_ = std::max(cost_right_goal_, cost_goal);
R_.points.push_back(point);
}
else
center_cost_ = std::max(center_cost_, cost_goal);
@ -1575,13 +1292,12 @@ void mkt_algorithm::diff::PredictiveTrajectory::publishMarkers(nav_2d_msgs::Pose
break;
}
}
if (enable_publish_)
{
cost_left_goals_pub_.publish(L_);
cost_right_goals_pub_.publish(R_);
}
R_.points.clear();
L_.points.clear();
}
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::PredictiveTrajectory, score_algorithm::ScoreAlgorithm)
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::PredictiveTrajectory::create()
{
return std::make_shared<mkt_algorithm::diff::PredictiveTrajectory>();
}
// Register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(mkt_algorithm::diff::PredictiveTrajectory::create, MKTAlgorithmDiffPredictiveTrajectory)

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

@ -1,25 +1,24 @@
#include <robot/console.h>
#include <mkt_algorithm/diff/diff_rotate_to_goal.h>
// other
#include <nav_2d_utils/parameters.h>
#include <pluginlib/class_list_macros.h>
#include <boost/dll/alias.hpp>
#include <angles/angles.h>
void mkt_algorithm::diff::RotateToGoal::initialize(
ros::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROS *costmap_ros, const score_algorithm::TrajectoryGenerator::Ptr &traj)
robot::NodeHandle &nh, std::string name, TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj)
{
if (!initialized_)
{
nh_ = ros::NodeHandle(nh, name);
nh_ = robot::NodeHandle(nh, name);
name_ = name;
tf_ = tf;
traj_ = traj;
costmap_ros_ = costmap_ros;
costmap_robot_ = costmap_robot;
this->getParams();
x_direction_ = y_direction_ = theta_direction_ = 0;
this->initialized_ = true;
ROS_INFO("RotateToGoal Initialized successfully");
robot::log_info("[%s:%d] RotateToGoal Initialized successfully", __FILE__, __LINE__);
}
}
@ -59,8 +58,7 @@ void mkt_algorithm::diff::RotateToGoal::getParams()
nh_priv_.param<double>("cost_scaling_gain", cost_scaling_gain_, 1.0);
if (inflation_cost_scaling_factor_ <= 0.0)
{
ROS_WARN("The value inflation_cost_scaling_factor is incorrectly set, "
"it should be >0. Disabling cost regulated linear velocity scaling.");
robot::log_warning("[%s:%d] The value inflation_cost_scaling_factor is incorrectly set, it should be >0. Disabling cost regulated linear velocity scaling.", __FILE__, __LINE__);
use_cost_regulated_linear_velocity_scaling_ = false;
}
double control_frequency = nav_2d_utils::searchAndGetParam(nh_priv_, "controller_frequency", 10);
@ -96,7 +94,7 @@ bool mkt_algorithm::diff::RotateToGoal::prepare(const nav_2d_msgs::Pose2DStamped
if (!initialized_)
{
ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
robot::log_error("This planner has not been initialized, please call initialize() before using this planner");
return false;
}
this->getParams();
@ -128,4 +126,9 @@ mkt_msgs::Trajectory2D mkt_algorithm::diff::RotateToGoal::calculator(
return result;
}
PLUGINLIB_EXPORT_CLASS(mkt_algorithm::diff::RotateToGoal, score_algorithm::ScoreAlgorithm)
score_algorithm::ScoreAlgorithm::Ptr mkt_algorithm::diff::RotateToGoal::create()
{
return std::make_shared<mkt_algorithm::diff::RotateToGoal>();
}
BOOST_DLL_ALIAS(mkt_algorithm::diff::RotateToGoal::create, MKTAlgorithmDiffRotateToGoal)

4
src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt
View File

@ -78,7 +78,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

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

@ -58,23 +58,23 @@ namespace two_points_planner
unsigned char result = 0;
// Bug
// if (!costmap_robot_)
// {
// std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
// return 0;
// }
if (!costmap_robot_)
{
std::cerr << "TwoPointsPlanner: Costmap is not initialized" << std::endl;
return 0;
}
// // 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();
// layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
// ++layer)
// {
// boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
// if (!inflation_layer)
// continue;
// 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();
layer != costmap_robot_->getLayeredCostmap()->getPlugins()->end();
++layer)
{
boost::shared_ptr<costmap_2d::InflationLayer> inflation_layer = boost::dynamic_pointer_cast<costmap_2d::InflationLayer>(*layer);
if (!inflation_layer)
continue;
// result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
// }
result = costMapCostToSBPLCost(inflation_layer->computeCost(costmap_robot_->getLayeredCostmap()->getCircumscribedRadius() / costmap_robot_->getCostmap()->getResolution()));
}
return result;
}
@ -109,104 +109,104 @@ namespace two_points_planner
return false;
}
// bool do_init = false;
// if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
// current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
// {
// printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
// current_env_width_, current_env_height_,
// costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
// do_init = true;
// }
// else if (footprint_ != costmap_robot_->getRobotFootprint())
// {
// printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
// do_init = true;
// }
bool do_init = false;
if (current_env_width_ != costmap_robot_->getCostmap()->getSizeInCellsX() ||
current_env_height_ != costmap_robot_->getCostmap()->getSizeInCellsY())
{
printf("TwoPointsPlanner: Costmap dimensions have changed from (%u x %u) to (%u x %u), reinitializing two_points_planner.\n",
current_env_width_, current_env_height_,
costmap_robot_->getCostmap()->getSizeInCellsX(), costmap_robot_->getCostmap()->getSizeInCellsY());
do_init = true;
}
else if (footprint_ != costmap_robot_->getRobotFootprint())
{
printf("TwoPointsPlanner: Robot footprint has changed, reinitializing two_points_planner.\n");
do_init = true;
}
// if (do_init)
// {
// initialized_ = false;
// initialize(name_, costmap_robot_);
// }
if (do_init)
{
initialized_ = false;
initialize(name_, costmap_robot_);
}
// plan.clear();
// plan.push_back(start);
// printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
// start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
plan.clear();
plan.push_back(start);
printf("TwoPointsPlanner getting start point (%.2f,%.2f) goal point (%.2f,%.2f)\n",
start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
// unsigned int mx_start, my_start;
// unsigned int mx_end, my_end;
// if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
// start.pose.position.y,
// mx_start, my_start)
unsigned int mx_start, my_start;
unsigned int mx_end, my_end;
if (!costmap_robot_->getCostmap()->worldToMap(start.pose.position.x,
start.pose.position.y,
mx_start, my_start)
// || !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
// goal.pose.position.y,
// mx_end, my_end))
// {
// std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
// return false;
// }
// 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))
// {
// std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
// return false;
// }
// robot::Time plan_time = robot::Time::now();
// // Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
// const double dx = goal.pose.position.x - start.pose.position.x;
// const double dy = goal.pose.position.y - start.pose.position.y;
// double distance = std::sqrt(dx * dx + dy * dy);
// double theta;
// if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
// {
// theta = atan2(dy, dx);
// tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y,
// goal.pose.orientation.z, goal.pose.orientation.w);
// double goal_theta = tf3::getYaw(goal_quat);
// if(cos(goal_theta - theta) < 0) theta += M_PI;
// }
// else
// {
// std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
// return false;
// }
|| !costmap_robot_->getCostmap()->worldToMap(goal.pose.position.x,
goal.pose.position.y,
mx_end, my_end))
{
std::cerr << "TwoPointsPlanner: can not convert world to Map 'start point' or 'goal point'" << std::endl;
return false;
}
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))
{
std::cerr << "[two_points_planner] base_pootprint in infated obstacle " << std::endl;
return false;
}
robot::Time plan_time = robot::Time::now();
// Tính toán khoảng cách giữa điểm bắt đầu và điểm đích
const double dx = goal.pose.position.x - start.pose.position.x;
const double dy = goal.pose.position.y - start.pose.position.y;
double distance = std::sqrt(dx * dx + dy * dy);
double theta;
if(fabs(dx) > 1e-9 || fabs(dy) > 1e-9)
{
theta = atan2(dy, dx);
tf3::Quaternion goal_quat(goal.pose.orientation.x, goal.pose.orientation.y,
goal.pose.orientation.z, goal.pose.orientation.w);
double goal_theta = tf3::getYaw(goal_quat);
if(cos(goal_theta - theta) < 0) theta += M_PI;
}
else
{
std::cerr << "[two_points_planner] can not calculating theta from 'start point' or 'goal point'" << std::endl;
return false;
}
// // Lấy độ phân giải của costmap
// double resolution = costmap_robot_->getCostmap()->getResolution();
// Lấy độ phân giải của costmap
double resolution = costmap_robot_->getCostmap()->getResolution();
// // Tính số điểm cần chia
// int num_points = std::ceil(distance / resolution);
// Tính số điểm cần chia
int num_points = std::ceil(distance / resolution);
// // Chia nhỏ tuyến đường
// for (int i = 0; i <= num_points; i++)
// {
// double fraction = static_cast<double>(i) / num_points;
// Chia nhỏ tuyến đường
for (int i = 0; i <= num_points; i++)
{
double fraction = static_cast<double>(i) / num_points;
// 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;
// pose.pose.position.y = start.pose.position.y + fraction * dy;
// pose.pose.position.z = start.pose.position.z;
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;
pose.pose.position.y = start.pose.position.y + fraction * dy;
pose.pose.position.z = start.pose.position.z;
// // Nội suy hướng
// tf3::Quaternion interpolated_quat;
// interpolated_quat.setRPY(0, 0, theta);
// Nội suy hướng
tf3::Quaternion interpolated_quat;
interpolated_quat.setRPY(0, 0, theta);
// // Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
// pose.pose.orientation.x = interpolated_quat.x();
// pose.pose.orientation.y = interpolated_quat.y();
// pose.pose.orientation.z = interpolated_quat.z();
// pose.pose.orientation.w = interpolated_quat.w();
// Chuyển đổi trực tiếp từ tf3::Quaternion sang geometry_msgs::Quaternion
pose.pose.orientation.x = interpolated_quat.x();
pose.pose.orientation.y = interpolated_quat.y();
pose.pose.orientation.z = interpolated_quat.z();
pose.pose.orientation.w = interpolated_quat.w();
// plan.push_back(pose);
// }
// plan.push_back(goal);
plan.push_back(pose);
}
plan.push_back(goal);
return true;
}

2
src/Libraries/common_msgs

@ -1 +1 @@
Subproject commit bf1fc3df34fa79be25cb0e99b1bdb441ec3c7a9d
Subproject commit 6bac68429824cab4bbb1df3e259a24a8bf741b59

2
src/Libraries/costmap_2d

@ -1 +1 @@
Subproject commit ddff75465ca49b04bf22d2fbe26f61aa4bff7acb
Subproject commit fdfba18bde0662ab5f042b938ecd011c5382ca7a

12
src/Libraries/nav_2d_utils/CMakeLists.txt
View File

@ -32,6 +32,8 @@ target_link_libraries(conversions
nav_msgs
nav_grid
nav_core2
robot::node_handle
robot::console
PRIVATE
console_bridge::console_bridge
Boost::system
@ -48,6 +50,8 @@ target_link_libraries(path_ops
PUBLIC
nav_2d_msgs
geometry_msgs
robot::node_handle
robot::console
)
add_library(polygons src/polygons.cpp src/footprint.cpp)
@ -60,6 +64,8 @@ target_link_libraries(polygons
PUBLIC
nav_2d_msgs
geometry_msgs
robot::node_handle
robot::console
PRIVATE
${xmlrpcpp_LIBRARIES}
)
@ -81,6 +87,8 @@ target_link_libraries(bounds
PUBLIC
nav_grid
nav_core2
robot::node_handle
robot::console
)
add_library(tf_help src/tf_help.cpp)
@ -95,6 +103,8 @@ target_link_libraries(tf_help
geometry_msgs
nav_core2
tf3
robot::node_handle
robot::console
)
# Create an INTERFACE library that represents all nav_2d_utils libraries
@ -111,6 +121,8 @@ target_link_libraries(nav_2d_utils
polygons
bounds
tf_help
robot::node_handle
robot::console
)
# Install header files

4
src/Libraries/nav_2d_utils/include/nav_2d_utils/footprint.h
View File

@ -35,7 +35,7 @@
#ifndef NAV_2D_UTILS_FOOTPRINT_H
#define NAV_2D_UTILS_FOOTPRINT_H
#include <yaml-cpp/yaml.h>
#include <robot/node_handle.h>
#include <nav_2d_msgs/Polygon2D.h>
namespace nav_2d_utils
@ -47,7 +47,7 @@ namespace nav_2d_utils
* Analagous to costmap_2d::makeFootprintFromParams in that it will return an empty polygon if neither parameter
* is present.
*/
nav_2d_msgs::Polygon2D footprintFromParams(YAML::Node& nh, bool write = true);
nav_2d_msgs::Polygon2D footprintFromParams(robot::NodeHandle& nh, bool write = true);
} // namespace nav_2d_utils

4
src/Libraries/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h
View File

@ -41,7 +41,7 @@
#include <boost/thread/mutex.hpp>
#include <string>
#include <iostream>
#include <yaml-cpp/yaml.h>
#include <robot/node_handle.h>
namespace nav_2d_utils
{
@ -59,7 +59,7 @@ public:
* @param nh NodeHandle for creating subscriber
* @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
*/
explicit OdomSubscriber(YAML::Node& nh, std::string default_topic = "odom")
explicit OdomSubscriber(robot::NodeHandle& nh, std::string default_topic = "odom")
{
std::string odom_topic;
// nh.param("odom_topic", odom_topic, default_topic);

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

@ -52,7 +52,7 @@ namespace nav_2d_utils
* @return Value of parameter if found, otherwise the default_value
*/
template<class param_t>
param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, const param_t& default_value)
param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
{
std::string resolved_name;
// if (nh.searchParam(param_name, resolved_name))
@ -73,19 +73,19 @@ param_t searchAndGetParam(const YAML::Node& nh, const std::string& param_name, c
* @return The value of the parameter or the default value
*/
template<class param_t>
param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string current_name,
param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::string current_name,
const std::string old_name, const param_t& default_value)
{
param_t value;
if (nh[current_name] && nh[current_name].IsDefined())
if (nh.hasParam(current_name))
{
value = nh[current_name].as<param_t>();
nh.getParam(current_name, value, default_value);
return value;
}
if (nh[old_name] && nh[old_name].IsDefined())
if (nh.hasParam(old_name))
{
std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
value = nh[old_name].as<param_t>();
nh.getParam(old_name, value, default_value);
return value;
}
return default_value;
@ -98,14 +98,14 @@ param_t loadParameterWithDeprecation(const YAML::Node& nh, const std::string cur
* @param old_name Deprecated parameter name
*/
template<class param_t>
void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_name, const std::string old_name)
void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string current_name, const std::string old_name)
{
if (!nh[old_name] || !nh[old_name].IsDefined()) return;
if (!nh.hasParam(old_name)) return;
param_t value;
std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
value = nh[old_name].as<param_t>();
nh[current_name] = value;
value = nh.param<param_t>(old_name);
nh.setParam(current_name, value);
}
/**
@ -122,7 +122,7 @@ void moveDeprecatedParameter(const YAML::Node& nh, const std::string current_nam
* @param should_delete If true, whether to delete the parameter from the old name
*/
template<class param_t>
void moveParameter(const YAML::Node& nh, std::string old_name,
void moveParameter(const robot::NodeHandle& nh, std::string old_name,
std::string current_name, param_t default_value, bool should_delete = true)
{
// if (nh.hasParam(current_name))

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

@ -207,7 +207,7 @@ protected:
// ROS Interface
ros::ServiceServer switch_plugin_srv_;
ros::Publisher current_plugin_pub_;
YAML::Node private_nh_;
robot::NodeHandle private_nh_;
std::string ros_name_;
// Switch Callback

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

@ -89,7 +89,7 @@ nav_2d_msgs::Polygon2D polygonFromString(const std::string& polygon_string);
// * @param search Whether to search up the namespace for the parameter name
// * @return Loaded polygon
// */
// nav_2d_msgs::Polygon2D polygonFromParams(const YAML::Node& nh, const std::string parameter_name,
// nav_2d_msgs::Polygon2D polygonFromParams(const robot::NodeHandle& nh, const std::string parameter_name,
// bool search = true);
/**
@ -116,7 +116,7 @@ XmlRpc::XmlRpcValue polygonToXMLRPC(const nav_2d_msgs::Polygon2D& polygon, bool
// * @param parameter_name Name of the parameter
// * @param array_of_arrays If true, write an array of arrays. Otherwise, write two parallel arrays
// */
// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const YAML::Node& nh, const std::string parameter_name,
// void polygonToParams(const nav_2d_msgs::Polygon2D& polygon, const robot::NodeHandle& nh, const std::string parameter_name,
// bool array_of_arrays = true);
/**

2
src/Libraries/robot_cpp

@ -1 +1 @@
Subproject commit e3df6935432bcfdb14367bc99c2d114c385992a0
Subproject commit 1974d0ddeea08a05b83363614d1a946db5dd07ee

2
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

@ -28,6 +28,8 @@ target_link_libraries(nav_core2 INTERFACE
tf3
nav_grid
nav_2d_msgs
robot::node_handle
robot::console
)
# Set include directories

4
src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h
View File

@ -35,13 +35,13 @@
#ifndef NAV_CORE2_COSTMAP_H
#define NAV_CORE2_COSTMAP_H
#include <robot/node_handle.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/common.h>
#include <nav_core2/bounds.h>
#include <boost/thread.hpp>
#include <memory>
#include <string>
#include <yaml-cpp/yaml.h>
namespace nav_core2
{
@ -71,7 +71,7 @@ public:
* @param name The namespace for the costmap
* @param tf A pointer to a transform listener
*/
virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {}
virtual void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) {}
inline unsigned char getCost(const unsigned int x, const unsigned int y)
{

2
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h
View File

@ -68,7 +68,7 @@ public:
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
virtual void initialize(const YAML::Node& parent, const std::string& name,
virtual void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, Costmap::Ptr costmap) = 0;
/**

2
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h
View File

@ -76,7 +76,7 @@ namespace nav_core2
// virtual void initialize(YAML::Node &parent, const std::string &name,
// TFListenerPtr tf, Costmap::Ptr costmap) = 0;
virtual void initialize(YAML::Node &parent, const std::string &name,
virtual void initialize(robot::NodeHandle &parent, const std::string &name,
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
/**

4
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@ -75,7 +75,9 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

2
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h
View File

@ -60,7 +60,7 @@ public:
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
// Standard Costmap Interface
void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override;
void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
nav_core2::Costmap::mutex_t* getMutex() override;
// NavGrid Interface

2
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h
View File

@ -55,7 +55,7 @@ public:
GlobalPlannerAdapter();
// Nav Core 2 Global Planner Interface
void initialize(const YAML::Node& parent, const std::string& name,
void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal) override;

5
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h
View File

@ -35,6 +35,7 @@
#ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#include <robot/node_handle.h>
#include <nav_core/base_local_planner.h>
#include <nav_core2/local_planner.h>
#include <nav_core_adapter/costmap_adapter.h>
@ -197,8 +198,8 @@ namespace nav_core_adapter
boost::recursive_mutex configuration_mutex_;
// std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
YAML::Node private_nh_;
YAML::Node adapter_nh_;
robot::NodeHandle private_nh_;
robot::NodeHandle adapter_nh_;
// void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
nav_core_adapter::NavCoreAdapterConfig last_config_;
nav_core_adapter::NavCoreAdapterConfig default_config_;

4
src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp
View File

@ -72,10 +72,10 @@ void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool
costmap_ = costmap_robot_->getCostmap();
}
void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf)
void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
{
// TODO: Implement this if needed
throw nav_core2::CostmapException("initialize with YAML::Node not implemented");
throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented");
}
nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()

2
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp
View File

@ -53,7 +53,7 @@ namespace nav_core_adapter
/**
* @brief Load the nav_core global planner and initialize it
*/
void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name,
void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
costmap_ = costmap;

37
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp
View File

@ -32,6 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <robot/console.h>
#include <nav_core_adapter/local_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core_adapter/shared_pointers.h>
@ -79,23 +80,22 @@ namespace nav_core_adapter
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
YAML::Node nh;
private_nh_ = YAML::Node("~");
adapter_nh_ = YAML::Node("~/" + name);
std::cout << "Adapter namespace: " << adapter_nh_["~"].as<std::string>() << std::endl;
robot::NodeHandle nh;
private_nh_ = robot::NodeHandle("~");
adapter_nh_ = robot::NodeHandle(private_nh_, name);
robot::log_info("Adapter namespace: %s", private_nh_.getNamespace().c_str());
std::string planner_name;
if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined())
if (adapter_nh_.hasParam("planner_name"))
{
planner_name = adapter_nh_["planner_name"].as<std::string>();
adapter_nh_.getParam("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
}
else
{
planner_name = nav_2d_utils::loadParameterWithDeprecation(
adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_["planner_name"] = planner_name;
adapter_nh_.setParam("planner_name", planner_name);
}
std::cout << "Loading plugin " << planner_name << std::endl;
robot::log_info("Loading plugin %s", planner_name.c_str());
// planner_ = planner_loader_.createInstance(planner_name);
std::string path_file_so;
@ -104,10 +104,10 @@ namespace nav_core_adapter
planner_ = planner_loader_();
if (!planner_)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
robot::log_error("Failed to load planner %s", planner_name.c_str());
exit(EXIT_FAILURE);
}
planner_->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(), tf_, costmap_robot_);
planner_->initialize(private_nh_, planner_name, tf_, costmap_robot_);
// odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
// dsrv_ = std::make_shared<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>>(configuration_mutex_, adapter_nh_);
@ -140,26 +140,17 @@ namespace nav_core_adapter
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
new_planner->initialize(private_nh_, planner_name, tf_, costmap_robot_);
if (planner_)
planner_.reset();
planner_ = new_planner;
last_config_.planner_name = planner_name;
std::cout << "Loaded new planner: " << planner_name << std::endl;
robot::log_info("Loaded new planner: %s", planner_name.c_str());
}
catch (const std::exception &ex)
{
std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl;
robot::log_error("Failed to load planner %s: %s", planner_name.c_str(), ex.what());
return false;
}
}

4
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@ -237,7 +237,9 @@ option(BUILD_TESTS "Build test programs" OFF)
# ========================================================
# Compiler Flags
# ========================================================
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")

337
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@ -73,201 +73,196 @@ void move_base::MoveBase::initialize(TFListenerPtr tf)
// NodeHandle("~") will automatically load YAML files from config directory
robot::NodeHandle nh("~");
private_nh_ = nh;
private_nh_.printAllParams();
recovery_trigger_ = PLANNING_R;
robot::NodeHandle nh1 = robot::NodeHandle(nh);
nh1.setParam("local_costmap/obstacles/enabled", false);
// nh.printAllParams();
// nh1.printAllParams();
// nh.printAllParams();
// get some parameters that will be global to the move base node
std::string global_planner, local_planner;
private_nh_.getParam("base_global_planner", global_planner, std::string(""));
printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
private_nh_.getParam("base_local_planner", local_planner, std::string(""));
printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
// // get some parameters that will be global to the move base node
// std::string global_planner, local_planner;
// private_nh_.getParam("base_global_planner", global_planner, std::string(""));
// printf("[%s:%d] global_planner: %s\n", __FILE__, __LINE__, global_planner.c_str());
// private_nh_.getParam("base_local_planner", local_planner, std::string(""));
// printf("[%s:%d] local_planner: %s\n", __FILE__, __LINE__, local_planner.c_str());
// Handle nested YAML nodes for costmap config
std::string robot_base_frame;
private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
std::string global_frame;
private_nh_.getParam("global_frame", global_frame, std::string("map"));
robot_base_frame_ = robot_base_frame;
global_frame_ = global_frame;
double planner_frequency;
private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
planner_frequency_ = planner_frequency;
double controller_frequency;
private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
controller_frequency_ = controller_frequency;
double planner_patience;
private_nh_.getParam("planner_patience", planner_patience, 5.0);
planner_patience_ = planner_patience;
private_nh_.getParam("controller_patience", controller_patience_, 15.0);
double max_planning_retries;
private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
max_planning_retries_ = max_planning_retries;
double oscillation_timeout;
private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
oscillation_timeout_ = oscillation_timeout;
double oscillation_distance;
private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
oscillation_distance_ = oscillation_distance;
// // Handle nested YAML nodes for costmap config
// std::string robot_base_frame;
// private_nh_.getParam("robot_base_frame", robot_base_frame, std::string("base_footprint"));
// std::string global_frame;
// private_nh_.getParam("global_frame", global_frame, std::string("map"));
// robot_base_frame_ = robot_base_frame;
// global_frame_ = global_frame;
// double planner_frequency;
// private_nh_.getParam("planner_frequency", planner_frequency, 0.0);
// planner_frequency_ = planner_frequency;
// double controller_frequency;
// private_nh_.getParam("controller_frequency", controller_frequency, 20.0);
// controller_frequency_ = controller_frequency;
// double planner_patience;
// private_nh_.getParam("planner_patience", planner_patience, 5.0);
// planner_patience_ = planner_patience;
// private_nh_.getParam("controller_patience", controller_patience_, 15.0);
// double max_planning_retries;
// private_nh_.getParam("max_planning_retries", max_planning_retries, -1.0);
// max_planning_retries_ = max_planning_retries;
// double oscillation_timeout;
// private_nh_.getParam("oscillation_timeout", oscillation_timeout, 0.0);
// oscillation_timeout_ = oscillation_timeout;
// double oscillation_distance;
// private_nh_.getParam("oscillation_distance", oscillation_distance, 0.5);
// oscillation_distance_ = oscillation_distance;
double original_xy_goal_tolerance;
private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
original_xy_goal_tolerance_ = original_xy_goal_tolerance;
double original_yaw_goal_tolerance;
private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
// double original_xy_goal_tolerance;
// private_nh_.getParam("xy_goal_tolerance", original_xy_goal_tolerance, 0.2);
// original_xy_goal_tolerance_ = original_xy_goal_tolerance;
// double original_yaw_goal_tolerance;
// private_nh_.getParam("yaw_goal_tolerance", original_yaw_goal_tolerance, 0.2);
// original_yaw_goal_tolerance_ = original_yaw_goal_tolerance;
// defined local planner name
private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
// // defined local planner name
// private_nh_.getParam("position_planner_name", position_planner_name_, std::string("mkt_algorithm/MKTPositionPlanner"));
// private_nh_.getParam("docking_planner_name", docking_planner_name_, std::string("mkt_algorithm/MKTDockingPlanner"));
// private_nh_.getParam("go_straight_planner_name", go_straight_planner_name_, std::string("mkt_algorithm/MKTGoStraightPlanner"));
// private_nh_.getParam("rotate_planner_name", rotate_planner_name_, std::string("mkt_algorithm/MKTRotatePlanner"));
// parameters of make_plan service
private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
min_approach_linear_velocity_ *= 1.2;
// set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// // parameters of make_plan service
// private_nh_.getParam("make_plan_clear_costmap", make_plan_clear_costmap_, false);
// private_nh_.getParam("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, false);
// private_nh_.getParam("min_approach_linear_velocity", min_approach_linear_velocity_, 0.0);
// min_approach_linear_velocity_ *= 1.2;
// // set up plan triple buffer
// planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
// set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
// // set up the planner's thread
// planner_thread_ = new boost::thread(boost::bind(&move_base::MoveBase::planThread, this));
// we'll assume the radius of the robot to be consistent with what's specified for the costmaps
// From config param
double inscribed_radius;
private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
double circumscribed_radius;
private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
inscribed_radius_ = inscribed_radius;
circumscribed_radius_ = circumscribed_radius;
private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
double conservative_reset_dist;
private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
conservative_reset_dist_ = conservative_reset_dist;
private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
// // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
// // From config param
// double inscribed_radius;
// private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2);
// double circumscribed_radius;
// private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3);
// inscribed_radius_ = inscribed_radius;
// circumscribed_radius_ = circumscribed_radius;
// private_nh_.getParam("clearing_radius", clearing_radius_, 0.0);
// double conservative_reset_dist;
// private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0);
// conservative_reset_dist_ = conservative_reset_dist;
// private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false);
// private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true);
// private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true);
// create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
planner_costmap_robot_->pause();
// initialize the global planner
try
{
planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so",
global_planner,
boost::dll::load_mode::append_decorations);
// // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
// planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_);
// planner_costmap_robot_->pause();
// // initialize the global planner
// try
// {
// planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
// "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so",
// global_planner,
// boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
robot::log_error("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
throw std::runtime_error("Failed to load global planner " + global_planner);
}
if(planner_->initialize(global_planner, planner_costmap_robot_))
robot::log_info("[%s:%d] Global planner initialized successfully", __FILE__, __LINE__);
else
robot::log_error("[%s:%d] Global planner initialized failed", __FILE__, __LINE__);
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d] EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + global_planner + " planner");
}
// create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
controller_costmap_robot_->pause();
// create a local planner
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
controller_loader_ =
boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
tc_ = controller_loader_();
if (!tc_)
{
robot::log_error("[%s:%d] ERROR: controller_loader_() returned nullptr", __FILE__, __LINE__);
throw std::runtime_error("Failed to load local planner " + local_planner);
}
// tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
}
catch (const std::exception &ex)
{
robot::log_error("[%s:%d] EXCEPTION in local planner: %s", __FILE__, __LINE__, ex.what());
throw std::runtime_error("Failed to create the " + local_planner + " planner");
}
// planner_ = planner_loader_();
// if (!planner_)
// {
// robot::printf_red("[%s:%d] ERROR: planner_loader_() returned nullptr\n", __FILE__, __LINE__);
// throw std::runtime_error("Failed to load global planner " + global_planner);
// }
// if(planner_->initialize(global_planner, planner_costmap_robot_))
// printf("[%s:%d] Global planner initialized successfully\n", __FILE__, __LINE__);
// else
// robot::printf_red("[%s:%d] Global planner initialized failed\n", __FILE__, __LINE__);
// }
// catch (const std::exception &ex)
// {
// printf("[%s:%d] EXCEPTION in global planner: %s\n", __FILE__, __LINE__, ex.what());
// throw std::runtime_error("Failed to create the " + global_planner + " planner");
// }
// // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
// controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_);
// controller_costmap_robot_->pause();
// // create a local planner
// try
// {
// std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so";
// controller_loader_ =
// boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
// path_file_so, "LocalPlannerAdapter", boost::dll::load_mode::append_decorations);
// tc_ = controller_loader_();
// if (!tc_)
// {
// robot::printf_red("[%s:%d] ERROR: controller_loader_() returned nullptr\n", __FILE__, __LINE__);
// throw std::runtime_error("Failed to load local planner " + local_planner);
// }
// // tc_->initialize(local_planner, tf_.get(), controller_costmap_robot_);
// }
// catch (const std::exception &ex)
// {
// printf("[%s:%d] EXCEPTION in local planner: %s\n", __FILE__, __LINE__, ex.what());
// throw std::runtime_error("Failed to create the " + local_planner + " planner");
// }
// // Start actively updating costmaps based on sensor data
// planner_costmap_robot_->start();
// controller_costmap_robot_->start();
// Start actively updating costmaps based on sensor data
planner_costmap_robot_->start();
controller_costmap_robot_->start();
// try
// {
// old_linear_fwd_ = tc_->getTwistLinear(true);
// old_linear_bwd_ = tc_->getTwistLinear(false);
try
{
old_linear_fwd_ = tc_->getTwistLinear(true);
old_linear_bwd_ = tc_->getTwistLinear(false);
// old_angular_fwd_ = tc_->getTwistAngular(true);
// old_angular_rev_ = tc_->getTwistAngular(false);
// }
// catch (const std::exception &e)
// {
// std::cerr << e.what() << '\n';
// }
old_angular_fwd_ = tc_->getTwistAngular(true);
old_angular_rev_ = tc_->getTwistAngular(false);
}
catch (const std::exception &e)
{
std::cerr << e.what() << '\n';
}
// // // advertise a service for getting a plan
// // make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
// // advertise a service for getting a plan
// make_plan_srv_ = private_nh.advertiseService("make_plan", &move_base::MoveBase::planService, this);
// // // advertise a service for clearing the costmaps
// // clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
// // advertise a service for clearing the costmaps
// clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &move_base::MoveBase::clearCostmapsService, this);
// // if we shutdown our costmaps when we're deactivated... we'll do that now
// if (shutdown_costmaps_)
// {
// planner_costmap_robot_->stop();
// controller_costmap_robot_->stop();
// }
// if we shutdown our costmaps when we're deactivated... we'll do that now
if (shutdown_costmaps_)
{
planner_costmap_robot_->stop();
controller_costmap_robot_->stop();
}
// // load any user specified recovery behaviors, and if that fails load the defaults
// if (!loadRecoveryBehaviors(private_nh_))
// {
// robot::printf_yellow("[%s:%d] Loading default recovery behaviors\n", __FILE__, __LINE__);
// loadDefaultRecoveryBehaviors();
// }
// load any user specified recovery behaviors, and if that fails load the defaults
if (!loadRecoveryBehaviors(private_nh_))
{
robot::log_warning("[%s:%d] Loading default recovery behaviors", __FILE__, __LINE__);
loadDefaultRecoveryBehaviors();
}
// // initially, we'll need to make a plan
// state_ = move_base::PLANNING;
// initially, we'll need to make a plan
state_ = move_base::PLANNING;
// // we'll start executing recovery behaviors at the beginning of our list
// recovery_index_ = 0;
// nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
// if (nav_feedback_)
// {
// nav_feedback_->navigation_state = move_base_core::State::PLANNING;
// nav_feedback_->is_ready = true;
// }
// else
// {
// robot::printf_red("[%s:%d] ERROR: nav_feedback_ is nullptr\n", __FILE__, __LINE__);
// }
// we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;
nav_feedback_ = std::make_shared<move_base_core::NavFeedback>();
if (nav_feedback_)
{
nav_feedback_->navigation_state = move_base_core::State::PLANNING;
nav_feedback_->is_ready = true;
}
else
{
robot::log_error("[%s:%d] ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__);
}
// initialized_ = true;
// printf("[%s:%d] ========== End: initialize() - SUCCESS ==========\n", __FILE__, __LINE__);
initialized_ = true;
robot::log_info("[%s:%d] ========== End: initialize() - SUCCESS ==========", __FILE__, __LINE__);
}
else
{
robot::printf_yellow("[%s:%d] Already initialized, skipping\n", __FILE__, __LINE__);
robot::log_warning("[%s:%d] Already initialized, skipping", __FILE__, __LINE__);
}
}