diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5d062ba7356d4c5030eda0e5f3a7a74b1088a8d4..9695ebd284469fa2c9893545110bd0117d33260d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(adc_nav_module)
+project(iri_adc_nav_module)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree iri_adc_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -17,7 +17,7 @@ find_package(iriutils REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
@@ -86,7 +86,7 @@ find_package(iriutils REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
- cfg/NavModule.cfg
+ cfg/ADCNavModule.cfg
cfg/ADCNavClient.cfg
cfg/ADCNavBtClient.cfg
)
@@ -100,13 +100,13 @@ generate_dynamic_reconfigure_options(
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
-set(module_name adc_nav_module)
-set(module_bt_name adc_nav_module_bt)
+set(module_name iri_adc_nav_module)
+set(module_bt_name iri_adc_nav_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree iri_adc_msgs
# DEPENDS system_lib
)
@@ -124,7 +124,6 @@ include_directories(${iriutils_INCLUDE_DIR})
# Module
add_library(${module_name}
src/adc_nav_module.cpp
- src/nav_module.cpp
)
target_link_libraries(${module_name} ${catkin_LIBRARIES})
diff --git a/cfg/ADCNavClient.cfg b/cfg/ADCNavClient.cfg
index 982a98f60a530e5b5f179ad9ab43b26ea20bab7c..a39277e031e56af4b5748e77452727ba91e65013 100755
--- a/cfg/ADCNavClient.cfg
+++ b/cfg/ADCNavClient.cfg
@@ -31,7 +31,7 @@
# Author:
-PACKAGE='adc_nav_module'
+PACKAGE='iri_adc_nav_module'
from dynamic_reconfigure.parameter_generator_catkin import *
@@ -39,9 +39,7 @@ gen = ParameterGenerator()
move_base = gen.add_group("Move base action")
costmaps = gen.add_group("Costmap")
-map = gen.add_group("Map")
-poi = gen.add_group("Got to point of interest action")
-waypoint = gen.add_group("Got to waypoints action")
+global_planner = gen.add_group("Global planner")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
@@ -52,9 +50,6 @@ move_base.add("mb_yaw", double_t, 0, "
move_base.add("mb_yaw_tol", double_t, 0, "Target Yaw tolerance", -1.0, -1.0, 1.0)
move_base.add("mb_frame_id", str_t, 0, "Target pose frame identifier", "map")
-map.add("map_name", str_t, 0, "Name of the map to use", "iri_map")
-map.add("map_change", bool_t, 0, "Update the desired map", False)
-
costmaps.add("cm_enable_auto_clear",bool_t, 0, "Enable costmaps autoclear", False)
costmaps.add("cm_disable_auto_clear",bool_t, 0, "Disable costmaps autoclear", False)
costmaps.add("cm_auto_clear_rate_hz",double_t,0, "Costmap autoclear rate", 0.1, 0.01, 1.0)
@@ -65,5 +60,9 @@ move_base.add("start_position_nav",bool_t, 0, "
move_base.add("start_orientation_nav",bool_t, 0, "Start navigating towards a orientation", False)
move_base.add("stop_nav", bool_t, 0, "Stop the current navigation", False)
move_base.add("print_goal_distance",bool_t, 0, "Print distance to goal", False)
+move_base.add("get_plan" ,bool_t, 0, "Get a plan from the current position to the goal position", False)
+
+global_planner.add("get_opendrive_map",bool_t,0, "Get the whole opendrive map", False)
+global_planner.add("get_opendrive_nodes",bool_t,0, "Get the opendrive nodes of the current plan", False)
exit(gen.generate(PACKAGE, "ADCNavClient", "ADCNavClient"))
diff --git a/cfg/NavModule.cfg b/cfg/ADCNavModule.cfg
similarity index 53%
rename from cfg/NavModule.cfg
rename to cfg/ADCNavModule.cfg
index 1a40824d806c22b026c87547aeac3d71f16fc6bc..74d6be7a6a3fc76adc3f3ca1425a6b91f0b240c1 100755
--- a/cfg/NavModule.cfg
+++ b/cfg/ADCNavModule.cfg
@@ -31,31 +31,14 @@
# Author:
-PACKAGE='adc_nav_module'
+PACKAGE='iri_adc_nav_module'
from dynamic_reconfigure.parameter_generator_catkin import *
-from iri_ros_tools.dyn_params import add_module_service_params,add_module_action_params,add_module_params
+from iri_adc_nav_module.dyn_params import add_nav_module_params
gen = ParameterGenerator()
-odom = gen.add_group("Odometry")
-tf = gen.add_group("TF")
+nav_module = add_nav_module_params(gen,"nav")
-# Name Type Reconfiguration level Description Default Min Max
-#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
-add_module_params(gen,"head_module")
-
-move_base_action=add_module_action_params(gen,"move_base")
-move_base_action.add("move_base_frame_id",str_t, 0, "Reference frame of the position goals","map")
-move_base_action.add("move_base_cancel_prev", bool_t, 0, "Cancel previous action", True)
-
-odom.add("odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50)
-
-tf.add("tf_timeout_time_s", double_t, 0, "Maximum time to wait for transform",5, 0.01, 50)
-
-costmap=add_module_service_params(gen,"clear_costmap")
-costmap.add("clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False)
-costmap.add("clear_costmap_auto_clear_rate_hz",double_t, 0, "Clear costmaps period", 0.1, 0.01, 1.0)
-
-exit(gen.generate(PACKAGE, "NavModuleAlgorithm", "NavModule"))
+exit(gen.generate(PACKAGE, "ADCNavModuleAlgorithm", "ADCNavModule"))
diff --git a/config/adc_nav_module_default.yaml b/config/adc_nav_module_default.yaml
index 31c469828f95999863e5ed9616984828ab3b0128..20f5b033e2eb0e914abe7deed376c99f72da9be1 100644
--- a/config/adc_nav_module_default.yaml
+++ b/config/adc_nav_module_default.yaml
@@ -9,20 +9,6 @@ move_base_frame_id: "map"
move_base_enabled: True
move_base_cancel_prev: True
-move_poi_max_retries: 1
-move_poi_feedback_watchdog_time_s: 1.0
-move_poi_enable_watchdog: True
-move_poi_timeout_s: 100.0
-move_poi_enable_timeout: True
-move_poi_enabled: True
-
-move_waypoint_max_retries: 1
-move_waypoint_feedback_watchdog_time_s: 1.0
-move_waypoint_enable_watchdog: True
-move_waypoint_timeout_s: 100.0
-move_waypoint_enable_timeout: True
-move_waypoint_enabled: True
-
odom_watchdog_time_s: 1.0
tf_timeout_time_s: 5.0
@@ -32,9 +18,7 @@ clear_costmaps_enable_auto_clear: False
clear_costmaps_auto_clear_rate_hz: 0.1
clear_costmaps_enabled: True
-change_map_max_retries: 1
-change_map_enabled: True
-
-set_op_mode_max_retries: 1
-set_op_mode_enabled: True
-
+make_plan_max_retries: 1.0
+make_plan_enable_auto_clear: False
+make_plan_auto_clear_rate_hz: 0.1
+make_plan_enabled: True
diff --git a/include/adc_nav_client_alg.h b/include/adc_nav_client_alg.h
index 1875297fa312a5f2211a23731d1c17d58669c572..eda469782814ec3325ac4aeeb9da79fafddef52f 100644
--- a/include/adc_nav_client_alg.h
+++ b/include/adc_nav_client_alg.h
@@ -25,7 +25,7 @@
#ifndef _adc_nav_client_alg_h_
#define _adc_nav_client_alg_h_
-#include <adc_nav_module/ADCNavClientConfig.h>
+#include <iri_adc_nav_module/ADCNavClientConfig.h>
//include humanoid_common_adc_nav_client_alg main library
@@ -54,7 +54,7 @@ class ADCNavClientAlgorithm
* Define a Config type with the ADCNavClientConfig. All driver implementations
* will then use the same variable type Config.
*/
- typedef adc_nav_module::ADCNavClientConfig Config;
+ typedef iri_adc_nav_module::ADCNavClientConfig Config;
/**
* \brief config variable
diff --git a/include/adc_nav_client_alg_node.h b/include/adc_nav_client_alg_node.h
index 801649c89bf1fc548d0c43c5381477fbebe3abac..1326e320f912da5e0dcda87a72b5c6d4eecba95c 100644
--- a/include/adc_nav_client_alg_node.h
+++ b/include/adc_nav_client_alg_node.h
@@ -34,7 +34,7 @@
// [nav server client headers]
-#include <adc_nav_module/adc_nav_module.h>
+#include <iri_adc_nav_module/adc_nav_module.h>
/**
* \brief IRI ROS Specific Algorithm Class
diff --git a/include/adc_nav_module/ackermann_lp_module.h b/include/adc_nav_module/ackermann_lp_module.h
deleted file mode 100644
index 6d79e29ba73a7a35f48f2df38662f1fe90c13260..0000000000000000000000000000000000000000
--- a/include/adc_nav_module/ackermann_lp_module.h
+++ /dev/null
@@ -1,614 +0,0 @@
-#ifndef _ACKERMANN_LP_MODULE_H
-#define _ACKERMANN_LP_MODULE_H
-
-// IRI ROS headers
-#include <iri_ros_tools/module_dyn_reconf.h>
-#include <adc_nav_module/nav_planner_module.h>
-
-/**
- * \brief
- *
- */
-template <class ModuleCfg>
-class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
-{
- public:
- /**
- * \brief Constructor
- *
- */
- CAckermannLPModule(const std::string &name,const std::string &name_space=std::string(""));
- /* parameter functions */
- /**
- * \brief
- *
- */
- dyn_reconf_status_t using_steer_angle(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t use_steer_angle(bool steer_angle);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t pruning_plan(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t prune_plan(bool enable);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t using_dead_zone(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t use_dead_zone(bool enable, double dead_zone=0.0);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_trans_vel_samples(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_trans_vel_samples(unsigned int samples);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_steer_angle_samples(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_steer_angle_samples(unsigned int samples);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_angular_el_samples(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_angular_vel_samples(unsigned int samples);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_cmd_vel_average_samples(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_cmd_vel_average_samples(unsigned int samples);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_odom_average_samples(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_odom_average_samples(unsigned int samples);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_heading_cost_points(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_heading_cost_points(unsigned int points);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_simulation_time(double &min,double &max);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_simulation_time(double &min,double &max);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_simulation_res(double &linear, double &angular);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_simulation_res(double &linear, double &angular);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_path_distance_cost(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_path_distance_cost(double cost);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_goal_distance_cost(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_goal_distance_cost(double cost);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_obstacle_cost(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_obstacle_cost(double cost);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_heading_cost(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_heading_cost(double cost);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_goal_tolerances(double &xy,double &yaw);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_goal_tolerances(double &xy,double &yaw);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_linear_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_linear_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_angular_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_angular_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_linear_max_accel(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_linear_max_accel(double max);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_angular_max_accel(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_angular_max_accel(double max);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_steer_angle_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_steer_angle_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_steer_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_steer_velocity_range(double &max,double &min);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_min_segment_length(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_min_segment_length(double min_lenght);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_axis_distance(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_axis_distance(double distance);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_wheel_distance(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_wheel_distance(double distance);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_wheel_radius(void);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_wheel_radius(double radius);
- /**
- * \brief Destructor
- *
- */
- ~CAckermannLPModule();
- };
-
- template <class ModuleCfg>
- CAckermannLPModule<ModuleCfg>::CAckermannLPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
- {
-
- }
-
- /* parameter functions */
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::using_steer_angle(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_steer_angle(bool steer_angle)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::pruning_plan(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::prune_plan(bool enable)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::using_dead_zone(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_dead_zone(bool enable, double dead_zone)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_trans_vel_samples(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_trans_vel_samples(unsigned int samples)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_angle_samples(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_angle_samples(unsigned int samples)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_el_samples(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_vel_samples(unsigned int samples)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_cmd_vel_average_samples(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_cmd_vel_average_samples(unsigned int samples)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_odom_average_samples(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_odom_average_samples(unsigned int samples)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_heading_cost_points(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_heading_cost_points(unsigned int points)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_simulation_time(double &min,double &max)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_simulation_time(double &min,double &max)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_simulation_res(double &linear, double &angular)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_simulation_res(double &linear, double &angular)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_path_distance_cost(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_path_distance_cost(double cost)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_goal_distance_cost(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_goal_distance_cost(double cost)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_obstacle_cost(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_obstacle_cost(double cost)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_heading_cost(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_heading_cost(double cost)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_goal_tolerances(double &xy,double &yaw)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_goal_tolerances(double &xy,double &yaw)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_linear_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_linear_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_linear_max_accel(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_linear_max_accel(double max)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_max_accel(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_max_accel(double max)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_angle_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_angle_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_velocity_range(double &max,double &min)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_min_segment_length(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_min_segment_length(double min_lenght)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_axis_distance(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_axis_distance(double distance)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_wheel_distance(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_wheel_distance(double distance)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_wheel_radius(void)
- {
-
- }
-
- template <class ModuleCfg>
- dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_wheel_radius(double radius)
- {
-
- }
-
- template <class ModuleCfg>
- CAckermannLPModule<ModuleCfg>::~CAckermannLPModule()
- {
-
- }
-
- #endif
diff --git a/include/adc_nav_module/nav_costmap_module.h b/include/adc_nav_module/nav_costmap_module.h
deleted file mode 100644
index 10b4a59f4b8f1ca46a8ef15f3e01fca01840b595..0000000000000000000000000000000000000000
--- a/include/adc_nav_module/nav_costmap_module.h
+++ /dev/null
@@ -1,249 +0,0 @@
-#ifndef _NAV_COSTMAP_MODULE_H
-#define _NAV_COSTMAP_MODULE_H
-
-#include <geometry_msgs/Point.h>
-
-// IRI ROS headers
-#include <iri_ros_tools/module_dyn_reconf.h>
-
-/**
- * \brief
- *
- */
-template <class ModuleCfg>
-class CNavCostmapModule
-
-{
- private:
- // dynamic reconfigure
- /**
- * \brief
- *
- */
- CModuleDynReconf<ModuleCfg> costmap_reconf;
-
- public:
- /**
- * \brief Constructor
- *
- */
- CNavCostmapModule(const std::string &name,const std::string &name_space=std::string(""));
- /* parameter functions */
- /**
- * \brief
- *
- */
- unsigned int get_width(void);
- /**
- * \brief
- *
- */
- void set_width(unsigned int width);
- /**
- * \brief
- *
- */
- unsigned int get_height(void);
- /**
- * \brief
- *
- */
- void set_height(unsigned int height);
- /**
- * \brief
- *
- */
- void get_footprint(std::vector<geometry_msgs::Point> &footprint);
- /**
- * \brief
- *
- */
- void set_footprint(std::vector<geometry_msgs::Point> &footprint);
- /**
- * \brief
- *
- */
- double get_transform_tolerance(void);
- /**
- * \brief
- *
- */
- void set_transform_tolerance(double tol);
- /**
- * \brief
- *
- */
- double get_update_frequency(void);
- /**
- * \brief
- *
- */
- void set_update_frequency(double freq);
- /**
- * \brief
- *
- */
- double get_publish_frequency(void);
- /**
- * \brief
- *
- */
- void set_publish_frequency(double freq);
- /**
- * \brief
- *
- */
- double get_resolution(void);
- /**
- * \brief
- *
- */
- void set_resolution(double res);
- /**
- * \brief
- *
- */
- void get_origin(double &x,double &y);
- /**
- * \brief
- *
- */
- void set_origin(double &x,double &y);
- /**
- * \brief
- *
- */
- double get_footprint_padding(void);
- /**
- * \brief
- *
- */
- void set_footprint_padding(double pad);
- /**
- * \brief Destructor
- *
- */
- ~CNavCostmapModule();
- };
-
-template <class ModuleCfg>
-CNavCostmapModule<ModuleCfg>::CNavCostmapModule(const std::string &name,const std::string &name_space) : costmap_reconf(name,name_space)
-{
-
-}
-
-template <class ModuleCfg>
-unsigned int CNavCostmapModule<ModuleCfg>::get_width(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_width(unsigned int width)
-{
-
-}
-
-template <class ModuleCfg>
-unsigned int CNavCostmapModule<ModuleCfg>::get_height(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_height(unsigned int height)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::get_footprint(std::vector<geometry_msgs::Point> &footprint)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_footprint(std::vector<geometry_msgs::Point> &footprint)
-{
-
-}
-
-template <class ModuleCfg>
-double CNavCostmapModule<ModuleCfg>::get_transform_tolerance(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_transform_tolerance(double tol)
-{
-
-}
-
-template <class ModuleCfg>
-double CNavCostmapModule<ModuleCfg>::get_update_frequency(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_update_frequency(double freq)
-{
-
-}
-
-template <class ModuleCfg>
-double CNavCostmapModule<ModuleCfg>::get_publish_frequency(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_publish_frequency(double freq)
-{
-
-}
-
-template <class ModuleCfg>
-double CNavCostmapModule<ModuleCfg>::get_resolution(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_resolution(double res)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::get_origin(double &x,double &y)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_origin(double &x,double &y)
-{
-
-}
-
-template <class ModuleCfg>
-double CNavCostmapModule<ModuleCfg>::get_footprint_padding(void)
-{
-
-}
-
-template <class ModuleCfg>
-void CNavCostmapModule<ModuleCfg>::set_footprint_padding(double pad)
-{
-
-}
-
-template <class ModuleCfg>
-CNavCostmapModule<ModuleCfg>::~CNavCostmapModule()
-{
-
-}
-
- #endif
diff --git a/include/adc_nav_module/nav_module.h b/include/adc_nav_module/nav_module.h
deleted file mode 100644
index e6358db518e2798f9403fe2bd1d8353c897711c6..0000000000000000000000000000000000000000
--- a/include/adc_nav_module/nav_module.h
+++ /dev/null
@@ -1,356 +0,0 @@
-#ifndef _NAV_MODULE_H
-#define _NAV_MODULE_H
-
-// IRI ROS headers
-#include <iri_ros_tools/module.h>
-#include <iri_ros_tools/module_action.h>
-#include <iri_ros_tools/module_service.h>
-#include <iri_ros_tools/module_dyn_reconf.h>
-#include <iri_ros_tools/watchdog.h>
-#include <iri_ros_tools/timeout.h>
-#include <iostream>
-#include <limits>
-
-// ROS headers
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/Path.h>
-#include <nav_msgs/GetPlan.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_ros/transform_listener.h>
-#include <move_base_msgs/MoveBaseAction.h>
-#include <std_srvs/Empty.h>
-
-#include <adc_nav_module/nav_costmap_module.h>
-
-// Dynamic reconfigure header
-#include <adc_nav_module/NavModuleConfig.h>
-
-/**
- * \brief Navigation module
- *
- */
-class CNavModule : public CModule<adc_nav_module::NavModuleConfig>
-{
- private:
- // basic navigation action
- /**
- * \brief
- *
- */
- CModuleAction<move_base_msgs::MoveBaseAction,adc_nav_module::NavModuleConfig> move_base_action;
- /**
- * \brief
- *
- */
- move_base_msgs::MoveBaseGoal move_base_goal;
- /**
- * \brief
- *
- */
- std::string goal_frame_id;
- /**
- * \brief
- *
- */
- CModuleService<nav_msgs::GetPlan,adc_nav_module::NavModuleConfig> make_plan_service;
- // dynamic reconfigure
- /**
- * \brief
- *
- */
- CModuleDynReconf<adc_nav_module::NavModuleConfig> move_base_reconf;
- /**
- * \brief
- *
- */
- adc_nav_module::NavModuleConfig config;
-
- // odometry
- /**
- * \brief
- *
- */
- CROSWatchdog odom_watchdog;
- /**
- * \brief
- *
- */
- ros::Subscriber odom_subscriber;
- /**
- * \brief
- *
- */
- void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
-
- /**
- * \brief
- *
- */
- ros::Subscriber path_subscriber;
- /**
- * \brief
- *
- */
- void path_callback(const nav_msgs::Path::ConstPtr& msg);
-
- /**
- * \brief
- *
- */
- nav_msgs::Path path_msg;
-
- /**
- * \brief
- *
- */
- bool path_available;
- /**
- * \brief
- *
- */
- geometry_msgs::PoseStamped current_pose;
-
- // tf listener
- /**
- * \brief
- *
- */
- tf2_ros::Buffer tf2_buffer;
- /**
- * \brief
- *
- */
- tf2_ros::TransformListener tf2_listener;
-
- // costmaps
- /**
- * \brief
- *
- */
- CModuleService<std_srvs::Empty,adc_nav_module::NavModuleConfig> clear_costmaps;
- /**
- * \brief
- *
- */
- ros::Timer clear_costmaps_timer;
- /**
- * \brief
- *
- */
- void clear_costmaps_call(const ros::TimerEvent& event);
- /**
- * \brief
- *
- */
- bool enable_auto_clear;
- /**
- * \brief
- *
- */
- CNavCostmapModule<adc_nav_module::NavModuleConfig> local_costmap;
- /**
- * \brief
- *
- */
- CNavCostmapModule<adc_nav_module::NavModuleConfig> global_costmap;
- protected:
- /**
- * \brief
- *
- */
- act_srv_status start_action(double x,double y,double yaw);
- /**
- * \brief
- *
- */
- void cancel_action(void);
- /**
- * \brief
- *
- */
- action_status get_action_status(void);
- /**
- * \brief
- *
- */
- std::string get_pose_frame_id(void);
- /**
- * \brief
- *
- */
- std::string get_goal_frame_id(void);
- /**
- * \brief
- *
- */
- virtual void state_machine(void)=0;
- /**
- * \brief
- *
- */
- void reconfigure_callback(adc_nav_module::NavModuleConfig &config, uint32_t level);
- /**
- * \brief
- *
- */
- bool transform_current_pose(void);
-
- public:
- /**
- * \brief Constructor
- *
- */
- CNavModule(const std::string &name,const std::string &name_space=std::string(""));
- /**
- * \brief
- *
- */
- bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance);
- // common functions
- /**
- * \brief Set the goal reference frame
- *
- * This functions sets the reference with used for all the position,
- * orientation and pose goals. By default this frame is set to /map.
- *
- * \param frame_id name of the new reference frame for the future goals.
- * This frame can be any that exist inside the TF tree
- * of the robot.
- */
- void set_goal_frame(std::string &frame_id);
- /**
- * \brief Clears the costmaps
- *
- * This functions can be used to clear the navigation costmapas at any time.
- * On success, this function will remove any ghost obstacles that remained
- * from previous detections. Calling this function when the auto-clearing
- * feature is enabled will have no effect.
- *
- * Clearing the costmaps may improve the robot's navigation in narrow spaces.
- *
- * \return a boolean indicating whether the costmaps have been cleared (true)
- * or not (false);
- */
- bool costmaps_clear(void);
- /**
- * \brief Enable the autoclear of the costmaps
- *
- * This functions enables a periodic clearing of the navigation costmaps at a
- * desired rate, which in general should be lower than 1 Hz. When this feature
- * is enabled, it will be no longer possible to clear the costmaps manually
- * with the costmaps_clear() function.
- *
- * \param rate_hz the desired clearing rate in Hz. This value should be less
- * than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
- *
- */
- void costmaps_enable_auto_clear(double rate_hz);
- /**
- * \brief Disable the autoclear feature
- *
- * This function disables the periodic clearing of the navigation costmaps.
- * After disabling this feature, the costmaps_clear() function may be used
- * to do so.
- */
- void costmaps_disable_auto_clear(void);
- /**
- * \brief Checks the status of the autoclear feature
- *
- * This functions checks whether the costmaos autoclear feature is enabled
- * or not.
- *
- * \return a boolean indicating whether the costmaps autoclear feature is
- * enabled (true) or not (false).
- */
- bool costamp_is_auto_clear_enabled(void);
- /**
- * \brief Function to get the current pose of the robot.
- *
- * \return The robot pose with respect to the map frame.
- */
- geometry_msgs::Pose getCurrentPose(void);
- /**
- * \brief Function to get the euclidian distance to the goal.
- *
- * \return The Euclidian distance to the goal. -1.0 when not calculated.
- */
- double getGoalDistance(void);
- /* parameter functions */
- /**
- * \brief
- *
- */
- dyn_reconf_status_t are_costmaps_shutdown(bool &shutdown);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t shutdown_costmaps(bool &shutdown);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_max_num_retries(unsigned int &retries);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_max_num_retries(unsigned int &retries);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_planner_frequency(double &freq);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_planner_frequency(double &freq);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_planner_patience(double &time);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_planner_patience(double &time);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_controller_frequency(double &freq);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_controller_frequency(double &freq);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t get_controller_patience(double &time);
- /**
- * \brief
- *
- */
- dyn_reconf_status_t set_controller_patience(double &time);
- /**
- * \brief
- *
- */
- const CNavCostmapModule<adc_nav_module::NavModuleConfig> &get_local_costmap(void) const;
- /**
- * \brief
- *
- */
- const CNavCostmapModule<adc_nav_module::NavModuleConfig> &get_global_costmap(void) const;
- /**
- * \brief Destructor
- *
- */
- ~CNavModule();
- };
-
- #endif
diff --git a/include/adc_nav_module/opendrive_gp_module.h b/include/adc_nav_module/opendrive_gp_module.h
deleted file mode 100644
index abccf5230d4ceeb8da0983339563b4c997de9ccf..0000000000000000000000000000000000000000
--- a/include/adc_nav_module/opendrive_gp_module.h
+++ /dev/null
@@ -1,170 +0,0 @@
-#ifndef _OPENDRIVE_GP_MODULE_H
-#define _OPENDRIVE_GP_MODULE_H
-
-// IRI ROS headers
-#include <iri_ros_tools/module_dyn_reconf.h>
-#include <adc_nav_module/nav_planner_module.h>
-
-typedef enum {OD_GP_DIST_COST=0,OD_GP_TIME_COST=1} cost_type_t;
-
-/**
- * \brief
- *
- */
-template <class ModuleCfg>
-class COpendriveGPModule : public CNavPlannerModule<ModuleCfg>
-{
- public:
- /**
- * \brief Constructor
- *
- */
- COpendriveGPModule(const std::string &name,const std::string &name_space=std::string(""));
- /* parameter functions */
- dyn_reconf_status_t is_multi_hypothesis(bool &enable);
- dyn_reconf_status_t multy_hypothesis(bool enable);
- dyn_reconf_status_t get_cost_type(cost_type_t &type);
- dyn_reconf_status_t set_cost_type(cost_type_t type);
- dyn_reconf_status_t get_opendrive_map(std::string &map_file);
- dyn_reconf_status_t set_opendrive_map(const std::string &map_file);
- dyn_reconf_status_t get_opendrive_frame(std::string &frame);
- dyn_reconf_status_t get_popendrive_frame(const std::string &frame);
- dyn_reconf_status_t get_distance_tolerance(double &tolerance);
- dyn_reconf_status_t set_distance_tolerance(double tolerance);
- dyn_reconf_status_t get_angle_tolerance(double &tolerance);
- dyn_reconf_status_t set_angle_tolerance(double tolerance);
- dyn_reconf_status_t get_resolution(double &resolution);
- dyn_reconf_status_t set_resolution(double resolution);
- dyn_reconf_status_t get_scale_factor(double &scale);
- dyn_reconf_status_t set_scale_factor(double scale);
- dyn_reconf_status_t get_min_road_length(double &length);
- dyn_reconf_status_t set_min_raod_length(double length);
- /**
- * \brief Destructor
- *
- */
- ~COpendriveGPModule();
- };
-
- template<class ModuleCfg>
- COpendriveGPModule<ModuleCfg>::COpendriveGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
- {
-
- }
-
- /* parameter functions */
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::is_multi_hypothesis(bool &enable)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::multy_hypothesis(bool enable)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_cost_type(cost_type_t &type)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_cost_type(cost_type_t type)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_opendrive_map(std::string &map_file)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_opendrive_map(const std::string &map_file)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_opendrive_frame(std::string &frame)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_popendrive_frame(const std::string &frame)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_distance_tolerance(double &tolerance)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_distance_tolerance(double tolerance)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_angle_tolerance(double &tolerance)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_angle_tolerance(double tolerance)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_resolution(double &resolution)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_resolution(double resolution)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_scale_factor(double &scale)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_scale_factor(double scale)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_min_road_length(double &length)
- {
-
- }
-
- template<class ModuleCfg>
- dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_min_raod_length(double length)
- {
-
- }
-
- template<class ModuleCfg>
- COpendriveGPModule<ModuleCfg>::~COpendriveGPModule()
- {
-
- }
-
- #endif
diff --git a/include/iri_adc_nav_module/ackermann_lp_module.h b/include/iri_adc_nav_module/ackermann_lp_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..4078439780a085710e16e04b454031341d255a74
--- /dev/null
+++ b/include/iri_adc_nav_module/ackermann_lp_module.h
@@ -0,0 +1,800 @@
+#ifndef _ACKERMANN_LP_MODULE_H
+#define _ACKERMANN_LP_MODULE_H
+
+// IRI ROS headers
+#include <iri_adc_nav_module/nav_planner_module.h>
+
+/**
+ * \brief
+ *
+ */
+template <class ModuleCfg>
+class CAckermannLPModule : public CNavPlannerModule<ModuleCfg>
+{
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CAckermannLPModule(const std::string &name,const std::string &name_space=std::string(""));
+ /* parameter functions */
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t using_steer_angle(bool &enable);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t use_steer_angle(bool steer_angle);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t pruning_plan(bool &enable);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t prune_plan(bool enable);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t using_dead_zone(bool &enable);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t use_dead_zone(bool enable, double dead_zone=0.0);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_trans_vel_samples(unsigned int &samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_trans_vel_samples(unsigned int samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_steer_angle_samples(unsigned int &samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_steer_angle_samples(unsigned int samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_angular_vel_samples(unsigned int &samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_angular_vel_samples(unsigned int samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_cmd_vel_average_samples(unsigned int &samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_cmd_vel_average_samples(unsigned int samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_odom_average_samples(unsigned int &samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_odom_average_samples(unsigned int samples);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_heading_cost_points(unsigned int &points);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_heading_cost_points(unsigned int points);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_simulation_time(double &min,double &max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_simulation_time(double &min,double &max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_simulation_res(double &linear, double &angular);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_simulation_res(double &linear, double &angular);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_path_distance_cost(double &cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_path_distance_cost(double cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_goal_distance_cost(double &cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_goal_distance_cost(double cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_obstacle_cost(double &cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_obstacle_cost(double cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_heading_cost(double &cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_heading_cost(double cost);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_goal_tolerances(double &xy,double &yaw);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_goal_tolerances(double &xy,double &yaw);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_linear_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_linear_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_angular_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_angular_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_linear_max_accel(double &max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_linear_max_accel(double max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_steer_max_accel(double &max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_steer_max_accel(double max);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_steer_angle_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_steer_angle_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_steer_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_steer_velocity_range(double &max,double &min);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_min_segment_length(double &min_length);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_min_segment_length(double min_length);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_axis_distance(double &distance);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_axis_distance(double distance);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_wheel_distance(double &distance);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_wheel_distance(double distance);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_wheel_radius(double &radius);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_wheel_radius(double radius);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~CAckermannLPModule();
+ };
+
+ template <class ModuleCfg>
+ CAckermannLPModule<ModuleCfg>::CAckermannLPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space)
+ {
+
+ }
+
+ /* parameter functions */
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::using_steer_angle(bool &enable)
+ {
+ if(this->planner_reconf.get_parameter("use_steer_angle_cmd",enable))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_steer_angle(bool steer_angle)
+ {
+ this->planner_reconf.set_parameter("use_steer_angle_cmd",steer_angle);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::pruning_plan(bool &enable)
+ {
+ if(this->planner_reconf.get_parameter("prune_plan",enable))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::prune_plan(bool enable)
+ {
+ this->planner_reconf.set_parameter("prune_plan",enable);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::using_dead_zone(bool &enable)
+ {
+ if(this->planner_reconf.get_parameter("use_trans_vel_deadzone",enable))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::use_dead_zone(bool enable, double dead_zone)
+ {
+ this->planner_reconf.set_parameter("use_trans_vel_deadzone",enable);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("trans_vel_deadzone",dead_zone);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_trans_vel_samples(unsigned int &samples)
+ {
+ if(this->planner_reconf.get_parameter("trans_vel_samples",samples))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_trans_vel_samples(unsigned int samples)
+ {
+ this->planner_reconf.set_parameter("trans_vel_samples",samples);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_angle_samples(unsigned int &samples)
+ {
+ if(this->planner_reconf.get_parameter("steer_angle_samples",samples))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_angle_samples(unsigned int samples)
+ {
+ this->planner_reconf.set_parameter("steer_angle_samples",samples);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_vel_samples(unsigned int &samples)
+ {
+ if(this->planner_reconf.get_parameter("angular_vel_samples",samples))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_vel_samples(unsigned int samples)
+ {
+ this->planner_reconf.set_parameter("angular_vel_samples",samples);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_cmd_vel_average_samples(unsigned int &samples)
+ {
+ if(this->planner_reconf.get_parameter("cmd_vel_avg",samples))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_cmd_vel_average_samples(unsigned int samples)
+ {
+ this->planner_reconf.set_parameter("cmd_vel_avg",samples);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_odom_average_samples(unsigned int &samples)
+ {
+ if(this->planner_reconf.get_parameter("odom_avg",samples))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_odom_average_samples(unsigned int samples)
+ {
+ this->planner_reconf.set_parameter("odom_avg",samples);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_heading_cost_points(unsigned int &points)
+ {
+ if(this->planner_reconf.get_parameter("heading_points",points))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_heading_cost_points(unsigned int points)
+ {
+ this->planner_reconf.set_parameter("heading_points",points);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_simulation_time(double &min,double &max)
+ {
+ if(this->planner_reconf.get_parameter("min_sim_time",min))
+ {
+ if(this->planner_reconf.get_parameter("max_sim_time",max))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_simulation_time(double &min,double &max)
+ {
+ this->planner_reconf.set_parameter("min_sim_time",min);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("max_sim_time",max);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_simulation_res(double &linear, double &angular)
+ {
+ if(this->planner_reconf.get_parameter("sim_granularity",linear))
+ {
+ if(this->planner_reconf.get_parameter("angular_sim_granularity",angular))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_simulation_res(double &linear, double &angular)
+ {
+ this->planner_reconf.set_parameter("sim_granularity",linear);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("angular_sim_granularity",angular);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_path_distance_cost(double &cost)
+ {
+ if(this->planner_reconf.get_parameter("path_distance_bias",cost))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_path_distance_cost(double cost)
+ {
+ this->planner_reconf.set_parameter("path_distance_bias",cost);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_goal_distance_cost(double &cost)
+ {
+ if(this->planner_reconf.get_parameter("goal_distance_bias",cost))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_goal_distance_cost(double cost)
+ {
+ this->planner_reconf.set_parameter("goal_distance_bias",cost);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_obstacle_cost(double &cost)
+ {
+ if(this->planner_reconf.get_parameter("occdist_scale",cost))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_obstacle_cost(double cost)
+ {
+ this->planner_reconf.set_parameter("occdist_scale",cost);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_heading_cost(double &cost)
+ {
+ if(this->planner_reconf.get_parameter("hdiff_scale",cost))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_heading_cost(double cost)
+ {
+ this->planner_reconf.set_parameter("hdiff_scale",cost);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_goal_tolerances(double &xy,double &yaw)
+ {
+ if(this->planner_reconf.get_parameter("xy_goal_tolerance",xy))
+ {
+ if(this->planner_reconf.get_parameter("yaw_goal_tolerance",yaw))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_goal_tolerances(double &xy,double &yaw)
+ {
+ this->planner_reconf.set_parameter("xy_goal_tolerance",xy);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("yaw_goal_tolerance",yaw);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_linear_velocity_range(double &max,double &min)
+ {
+ if(this->planner_reconf.get_parameter("max_trans_vel",max))
+ {
+ if(this->planner_reconf.get_parameter("min_trans_vel",min))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_linear_velocity_range(double &max,double &min)
+ {
+ this->planner_reconf.set_parameter("max_trans_vel",max);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("min_trans_vel",min);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_angular_velocity_range(double &max,double &min)
+ {
+ if(this->planner_reconf.get_parameter("max_angular_vel",max))
+ {
+ if(this->planner_reconf.get_parameter("min_angular_vel",min))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_angular_velocity_range(double &max,double &min)
+ {
+ this->planner_reconf.set_parameter("max_angular_vel",max);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("min_angular_vel",min);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_linear_max_accel(double &max)
+ {
+ if(this->planner_reconf.get_parameter("max_trans_acc",max))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_linear_max_accel(double max)
+ {
+ this->planner_reconf.set_parameter("max_trans_acc",max);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_max_accel(double &max)
+ {
+ if(this->planner_reconf.get_parameter("max_steer_acc",max))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_max_accel(double max)
+ {
+ this->planner_reconf.set_parameter("max_steer_acc",max);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_angle_range(double &max,double &min)
+ {
+ if(this->planner_reconf.get_parameter("max_steer_angle",max))
+ {
+ if(this->planner_reconf.get_parameter("min_steer_angle",min))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_angle_range(double &max,double &min)
+ {
+ this->planner_reconf.set_parameter("max_steer_angle",max);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("min_steer_angle",min);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_steer_velocity_range(double &max,double &min)
+ {
+ if(this->planner_reconf.get_parameter("max_steer_vel",max))
+ {
+ if(this->planner_reconf.get_parameter("min_steer_vel",min))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_steer_velocity_range(double &max,double &min)
+ {
+ this->planner_reconf.set_parameter("max_steer_vel",max);
+ if(this->planner_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->planner_reconf.set_parameter("min_steer_vel",min);
+ return this->planner_reconf.get_status();
+ }
+ else
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_min_segment_length(double &min_length)
+ {
+ if(this->planner_reconf.get_parameter("split_ignore_length",min_length))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_min_segment_length(double min_length)
+ {
+ this->planner_reconf.set_parameter("split_ignore_length",min_length);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_axis_distance(double &distance)
+ {
+ if(this->planner_reconf.get_parameter("axis_distance",distance))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_axis_distance(double distance)
+ {
+ this->planner_reconf.set_parameter("axis_distance",distance);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_wheel_distance(double &distance)
+ {
+ if(this->planner_reconf.get_parameter("wheel_distance",distance))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_wheel_distance(double distance)
+ {
+ this->planner_reconf.set_parameter("wheel_distance",distance);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::get_wheel_radius(double &radius)
+ {
+ if(this->planner_reconf.get_parameter("wheel_radius",radius))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CAckermannLPModule<ModuleCfg>::set_wheel_radius(double radius)
+ {
+ this->planner_reconf.set_parameter("wheel_radius",radius);
+ return this->planner_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ CAckermannLPModule<ModuleCfg>::~CAckermannLPModule()
+ {
+
+ }
+
+ #endif
diff --git a/include/adc_nav_module/adc_nav_bt_module.h b/include/iri_adc_nav_module/adc_nav_bt_module.h
similarity index 100%
rename from include/adc_nav_module/adc_nav_bt_module.h
rename to include/iri_adc_nav_module/adc_nav_bt_module.h
diff --git a/include/adc_nav_module/adc_nav_module.h b/include/iri_adc_nav_module/adc_nav_module.h
similarity index 94%
rename from include/adc_nav_module/adc_nav_module.h
rename to include/iri_adc_nav_module/adc_nav_module.h
index e5c35bd030f017b282a252ec09a5fcb99b0334aa..032a20bf545b46641344fca0cee92d84cadf7fa4 100644
--- a/include/adc_nav_module/adc_nav_module.h
+++ b/include/iri_adc_nav_module/adc_nav_module.h
@@ -2,12 +2,12 @@
#define _ADC_NAV_MODULE_H
// IRI ROS headers
-#include <adc_nav_module/nav_module.h>
-#include <adc_nav_module/ackermann_lp_module.h>
-#include <adc_nav_module/opendrive_gp_module.h>
+#include <iri_adc_nav_module/nav_module.h>
+#include <iri_adc_nav_module/ackermann_lp_module.h>
+#include <iri_adc_nav_module/opendrive_gp_module.h>
// Dynamic reconfigure header
-#include <adc_nav_module/NavModuleConfig.h>
+#include <iri_adc_nav_module/ADCNavModuleConfig.h>
//States
typedef enum {ADC_NAV_MODULE_IDLE,
@@ -66,11 +66,12 @@ typedef enum {ADC_NAV_MODULE_RUNNING,
* \brief
*
*/
-class CADCNavModule : public CNavModule,
- CAckermannLPModule<adc_nav_module::NavModuleConfig>,
- COpendriveGPModule<adc_nav_module::NavModuleConfig>
+class CADCNavModule : public CNavModule<iri_adc_nav_module::ADCNavModuleConfig>,
+ public CAckermannLPModule<iri_adc_nav_module::ADCNavModuleConfig>,
+ public COpendriveGPModule<iri_adc_nav_module::ADCNavModuleConfig>
{
private:
+ iri_adc_nav_module::ADCNavModuleConfig config;
//Variables
/**
* \brief
@@ -114,7 +115,11 @@ class CADCNavModule : public CNavModule,
*
*/
void state_machine(void);
-
+ /**
+ * \brief
+ *
+ */
+ void reconfigure_callback(iri_adc_nav_module::ADCNavModuleConfig &config, uint32_t level);
public:
/**
* \brief Constructor
diff --git a/include/iri_adc_nav_module/nav_costmap_module.h b/include/iri_adc_nav_module/nav_costmap_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..5a54854a56ab926ad8305bfbaf0b0f2f441cbc7a
--- /dev/null
+++ b/include/iri_adc_nav_module/nav_costmap_module.h
@@ -0,0 +1,292 @@
+#ifndef _NAV_COSTMAP_MODULE_H
+#define _NAV_COSTMAP_MODULE_H
+
+#include <geometry_msgs/Point.h>
+
+// IRI ROS headers
+#include <iri_ros_tools/module_dyn_reconf.h>
+
+/**
+ * \brief
+ *
+ */
+template <class ModuleCfg>
+class CNavCostmapModule
+
+{
+ private:
+ // dynamic reconfigure
+ /**
+ * \brief
+ *
+ */
+ CModuleDynReconf<ModuleCfg> costmap_reconf;
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CNavCostmapModule(const std::string &name,const std::string &name_space=std::string(""));
+ /* parameter functions */
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_width(unsigned int &width);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_width(unsigned int width);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_height(unsigned int &height);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_height(unsigned int height);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_footprint(std::vector<geometry_msgs::Point> &footprint);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_footprint(std::vector<geometry_msgs::Point> &footprint);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_transform_tolerance(double &tol);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_transform_tolerance(double tol);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_update_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_update_frequency(double freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_publish_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_publish_frequency(double freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_resolution(double &res);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_resolution(double res);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_origin(double &x,double &y);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_origin(double &x,double &y);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_footprint_padding(double &pad);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_footprint_padding(double pad);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~CNavCostmapModule();
+ };
+
+ template <class ModuleCfg>
+ CNavCostmapModule<ModuleCfg>::CNavCostmapModule(const std::string &name,const std::string &name_space) : costmap_reconf("dyn_reconf",name_space+"/"+name)
+ {
+
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_width(unsigned int &width)
+ {
+ if(this->costmap_reconf.get_parameter("width",width))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_width(unsigned int width)
+ {
+ this->costmap_reconf.set_parameter("width",width);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_height(unsigned int &height)
+ {
+ if(this->costmap_reconf.get_parameter("height",height))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_height(unsigned int height)
+ {
+ this->costmap_reconf.set_parameter("height",height);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_footprint(std::vector<geometry_msgs::Point> &footprint)
+ {
+
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_footprint(std::vector<geometry_msgs::Point> &footprint)
+ {
+
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_transform_tolerance(double &tol)
+ {
+ if(this->costmap_reconf.get_parameter("transform_tolerance",tol))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_transform_tolerance(double tol)
+ {
+ this->costmap_reconf.set_parameter("transform_tolerance",tol);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_update_frequency(double &freq)
+ {
+ if(this->costmap_reconf.get_parameter("update_frequency",freq))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_update_frequency(double freq)
+ {
+ this->costmap_reconf.set_parameter("update_frequency",freq);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_publish_frequency(double &freq)
+ {
+ if(this->costmap_reconf.get_parameter("publish_frequency",freq))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_publish_frequency(double freq)
+ {
+ this->costmap_reconf.set_parameter("publish_frequency",freq);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_resolution(double &res)
+ {
+ if(this->costmap_reconf.get_parameter("resolution",res))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_resolution(double res)
+ {
+ this->costmap_reconf.set_parameter("resolution",res);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_origin(double &x,double &y)
+ {
+ if(this->costmap_reconf.get_parameter("origin_x",x))
+ {
+ if(this->costmap_reconf.get_parameter("origin_y",y))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_origin(double &x,double &y)
+ {
+ this->costmap_reconf.set_parameter("origin_x",x);
+ if(this->costmap_reconf.get_status()==DYN_RECONF_SUCCESSFULL)
+ {
+ this->costmap_reconf.set_parameter("origin_y",y);
+ return this->costmap_reconf.get_status();
+ }
+ else
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_footprint_padding(double &pad)
+ {
+ if(this->costmap_reconf.get_parameter("footprint_padding",pad))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::set_footprint_padding(double pad)
+ {
+ this->costmap_reconf.set_parameter("footprint_padding",pad);
+ return this->costmap_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ CNavCostmapModule<ModuleCfg>::~CNavCostmapModule()
+ {
+
+ }
+
+ #endif
diff --git a/include/iri_adc_nav_module/nav_module.h b/include/iri_adc_nav_module/nav_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..40c240473c0cba0409fce163395384ac4bcb268b
--- /dev/null
+++ b/include/iri_adc_nav_module/nav_module.h
@@ -0,0 +1,879 @@
+#ifndef _NAV_MODULE_H
+#define _NAV_MODULE_H
+
+// IRI ROS headers
+#include <iri_ros_tools/module.h>
+#include <iri_ros_tools/module_action.h>
+#include <iri_ros_tools/module_service.h>
+#include <iri_ros_tools/module_dyn_reconf.h>
+#include <iri_ros_tools/watchdog.h>
+#include <iri_ros_tools/timeout.h>
+#include <iostream>
+#include <limits>
+
+// ROS headers
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <nav_msgs/GetPlan.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
+#include <move_base_msgs/MoveBaseAction.h>
+#include <std_srvs/Empty.h>
+
+#include <iri_adc_nav_module/nav_costmap_module.h>
+
+/**
+ * \brief Navigation module
+ *
+ */
+template <class ModuleCfg>
+class CNavModule : public CModule<ModuleCfg>
+{
+ private:
+ // basic navigation action
+ /**
+ * \brief
+ *
+ */
+ CModuleAction<move_base_msgs::MoveBaseAction,ModuleCfg> move_base_action;
+ /**
+ * \brief
+ *
+ */
+ move_base_msgs::MoveBaseGoal move_base_goal;
+ /**
+ * \brief
+ *
+ */
+ bool move_base_cancel_prev;
+ /**
+ * \brief
+ *
+ */
+ std::string goal_frame_id;
+ /**
+ * \brief
+ *
+ */
+ CModuleService<nav_msgs::GetPlan,ModuleCfg> make_plan_service;
+ // dynamic reconfigure
+ /**
+ * \brief
+ *
+ */
+ CModuleDynReconf<ModuleCfg> move_base_reconf;
+ // odometry
+ /**
+ * \brief
+ *
+ */
+ CROSWatchdog odom_watchdog;
+ /**
+ * \brief
+ *
+ */
+ double odom_watchdog_time_s;
+ /**
+ * \brief
+ *
+ */
+ ros::Subscriber odom_subscriber;
+ /**
+ * \brief
+ *
+ */
+ void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+
+ /**
+ * \brief
+ *
+ */
+ ros::Subscriber path_subscriber;
+ /**
+ * \brief
+ *
+ */
+ void path_callback(const nav_msgs::Path::ConstPtr& msg);
+
+ /**
+ * \brief
+ *
+ */
+ nav_msgs::Path path_msg;
+
+ /**
+ * \brief
+ *
+ */
+ bool path_available;
+ /**
+ * \brief
+ *
+ */
+ geometry_msgs::PoseStamped current_pose;
+
+ // tf listener
+ /**
+ * \brief
+ *
+ */
+ tf2_ros::Buffer tf2_buffer;
+ /**
+ * \brief
+ *
+ */
+ tf2_ros::TransformListener tf2_listener;
+ /**
+ * \brief
+ *
+ */
+ double tf_timeout_time_s;
+ // costmaps
+ /**
+ * \brief
+ *
+ */
+ CModuleService<std_srvs::Empty,ModuleCfg> clear_costmaps;
+ /**
+ * \brief
+ *
+ */
+ ros::Timer clear_costmaps_timer;
+ /**
+ * \brief
+ *
+ */
+ void clear_costmaps_call(const ros::TimerEvent& event);
+ /**
+ * \brief
+ *
+ */
+ bool enable_auto_clear;
+ /**
+ * \brief
+ *
+ */
+ double auto_clear_rate_hz;
+ /**
+ * \brief
+ *
+ */
+ CNavCostmapModule<ModuleCfg> local_costmap;
+ /**
+ * \brief
+ *
+ */
+ CNavCostmapModule<ModuleCfg> global_costmap;
+ protected:
+ /**
+ * \brief
+ *
+ */
+ act_srv_status start_action(double x,double y,double yaw);
+ /**
+ * \brief
+ *
+ */
+ void cancel_action(void);
+ /**
+ * \brief
+ *
+ */
+ action_status get_action_status(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_pose_frame_id(void);
+ /**
+ * \brief
+ *
+ */
+ std::string get_goal_frame_id(void);
+ /**
+ * \brief
+ *
+ */
+ virtual void state_machine(void)=0;
+ /**
+ * \brief
+ *
+ */
+ virtual void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+ /**
+ * \brief
+ *
+ */
+ bool transform_current_pose(void);
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ CNavModule(const std::string &name,const std::string &name_space=std::string(""));
+ /**
+ * \brief
+ *
+ */
+ void dynamic_reconfigure(ModuleCfg &config, const std::string &name);
+ /**
+ * \brief
+ *
+ */
+ bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance,nav_msgs::Path &plan);
+ // common functions
+ /**
+ * \brief Set the goal reference frame
+ *
+ * This functions sets the reference with used for all the position,
+ * orientation and pose goals. By default this frame is set to /map.
+ *
+ * \param frame_id name of the new reference frame for the future goals.
+ * This frame can be any that exist inside the TF tree
+ * of the robot.
+ */
+ void set_goal_frame_id(std::string &frame_id);
+ /**
+ * \brief Clears the costmaps
+ *
+ * This functions can be used to clear the navigation costmapas at any time.
+ * On success, this function will remove any ghost obstacles that remained
+ * from previous detections. Calling this function when the auto-clearing
+ * feature is enabled will have no effect.
+ *
+ * Clearing the costmaps may improve the robot's navigation in narrow spaces.
+ *
+ * \return a boolean indicating whether the costmaps have been cleared (true)
+ * or not (false);
+ */
+ bool costmaps_clear(void);
+ /**
+ * \brief Enable the autoclear of the costmaps
+ *
+ * This functions enables a periodic clearing of the navigation costmaps at a
+ * desired rate, which in general should be lower than 1 Hz. When this feature
+ * is enabled, it will be no longer possible to clear the costmaps manually
+ * with the costmaps_clear() function.
+ *
+ * \param rate_hz the desired clearing rate in Hz. This value should be less
+ * than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
+ *
+ */
+ void costmaps_enable_auto_clear(double rate_hz);
+ /**
+ * \brief Disable the autoclear feature
+ *
+ * This function disables the periodic clearing of the navigation costmaps.
+ * After disabling this feature, the costmaps_clear() function may be used
+ * to do so.
+ */
+ void costmaps_disable_auto_clear(void);
+ /**
+ * \brief Checks the status of the autoclear feature
+ *
+ * This functions checks whether the costmaos autoclear feature is enabled
+ * or not.
+ *
+ * \return a boolean indicating whether the costmaps autoclear feature is
+ * enabled (true) or not (false).
+ */
+ bool costamp_is_auto_clear_enabled(void);
+ /**
+ * \brief Function to get the current pose of the robot.
+ *
+ * \return The robot pose with respect to the map frame.
+ */
+ geometry_msgs::Pose get_current_pose(void);
+ /**
+ * \brief Function to get the euclidian distance to the goal.
+ *
+ * \return The Euclidian distance to the goal. -1.0 when not calculated.
+ */
+ double getGoalDistance(void);
+ /* parameter functions */
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t are_costmaps_shutdown(bool &shutdown);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t shutdown_costmaps(bool &shutdown);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_max_num_retries(unsigned int &retries);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_max_num_retries(unsigned int &retries);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_planner_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_planner_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_planner_patience(double &time);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_planner_patience(double &time);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_controller_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_controller_frequency(double &freq);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t get_controller_patience(double &time);
+ /**
+ * \brief
+ *
+ */
+ dyn_reconf_status_t set_controller_patience(double &time);
+ /**
+ * \brief
+ *
+ */
+ const CNavCostmapModule<ModuleCfg> &get_local_costmap(void) const;
+ /**
+ * \brief
+ *
+ */
+ const CNavCostmapModule<ModuleCfg> &get_global_costmap(void) const;
+ /**
+ * \brief Destructor
+ *
+ */
+ ~CNavModule();
+ };
+
+ template <class ModuleCfg>
+ CNavModule<ModuleCfg>::CNavModule(const std::string &name,const std::string &name_space) : CModule<ModuleCfg>(name,name_space),
+ tf2_listener(tf2_buffer),
+ move_base_action("move_base",this->module_nh.getNamespace()),
+ move_base_reconf("move_base_reconf",this->module_nh.getNamespace()),
+ make_plan_service("make_plan",this->module_nh.getNamespace()),
+ clear_costmaps("clear_costmaps",this->module_nh.getNamespace()),
+ local_costmap("local_costmap",this->module_nh.getNamespace()),
+ global_costmap("global_costmap",this->module_nh.getNamespace())
+ {
+ //Variables
+ this->path_available=false;
+ this->move_base_cancel_prev=false;
+ this->odom_watchdog_time_s=1.0;
+ this->tf_timeout_time_s=1.0;
+
+ // initialize odometry subscriber
+ this->odom_subscriber = this->module_nh.subscribe("odom",1,&CNavModule::odom_callback,this);
+
+ // initialize path subscriber
+ this->path_subscriber = this->module_nh.subscribe("path", 1, &CNavModule::path_callback, this);
+
+ // costmaps
+ this->enable_auto_clear=false;
+ this->auto_clear_rate_hz=10.0;
+ this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->auto_clear_rate_hz),&CNavModule::clear_costmaps_call,this);
+ this->clear_costmaps_timer.stop();
+ }
+
+ template <class ModuleCfg>
+ std::string CNavModule<ModuleCfg>::get_pose_frame_id(void)
+ {
+ return this->current_pose.header.frame_id;
+ }
+
+ template <class ModuleCfg>
+ std::string CNavModule<ModuleCfg>::get_goal_frame_id(void)
+ {
+ return this->goal_frame_id;
+ }
+
+ template <class ModuleCfg>
+ act_srv_status CNavModule<ModuleCfg>::start_action(double x,double y,double yaw)
+ {
+ tf2::Quaternion quat;
+ act_srv_status status;
+
+ this->move_base_goal.target_pose.header.stamp=ros::Time::now();
+ this->move_base_goal.target_pose.header.frame_id=this->goal_frame_id;
+ this->move_base_goal.target_pose.pose.position.x=x;
+ this->move_base_goal.target_pose.pose.position.y=y;
+ this->move_base_goal.target_pose.pose.position.z=0.0;
+ quat.setRPY(0.0,0.0,yaw);
+ this->move_base_goal.target_pose.pose.orientation=tf2::toMsg(quat);
+ this->path_available=false;
+ if(this->move_base_cancel_prev && !this->move_base_action.is_finished())
+ this->move_base_action.cancel();
+ status=this->move_base_action.make_request(this->move_base_goal);
+
+ return status;
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::cancel_action(void)
+ {
+ this->move_base_action.cancel();
+ }
+
+ template <class ModuleCfg>
+ action_status CNavModule<ModuleCfg>::get_action_status(void)
+ {
+ action_status status;
+
+ status=this->move_base_action.get_state();
+ if(status!=ACTION_IDLE && status!=ACTION_RUNNING)
+ this->move_base_action.stop_timeout();
+ if(status==ACTION_TIMEOUT || status==ACTION_FB_WATCHDOG)
+ this->move_base_action.cancel();
+
+ return status;
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+ {
+ ROS_DEBUG("CNavModule: odometry callback");
+
+ this->lock();
+ // reset the odometry callback
+ this->odom_watchdog.reset(ros::Duration(this->odom_watchdog_time_s));
+ // save the current position of the robot
+ this->current_pose.pose=msg->pose.pose;
+ this->current_pose.header=msg->header;
+ this->unlock();
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::path_callback(const nav_msgs::Path::ConstPtr& msg)
+ {
+ ROS_DEBUG("CNavModule: path callback");
+
+ this->lock();
+ this->path_msg = *msg;
+ this->path_available = true;
+ this->unlock();
+ }
+
+ template <class ModuleCfg>
+ bool CNavModule<ModuleCfg>::transform_current_pose(void)
+ {
+ geometry_msgs::TransformStamped transform;
+ geometry_msgs::PoseStamped pose;
+ bool tf2_exists;
+
+ try{
+ if(!this->odom_watchdog.is_active())
+ {
+ tf2_exists=this->tf2_buffer.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp,ros::Duration(this->tf_timeout_time_s));
+ transform = this->tf2_buffer.lookupTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp);
+ if(tf2_exists)
+ {
+ tf2::doTransform(this->current_pose,pose,transform);
+ if(!this->odom_watchdog.is_active())
+ {
+ this->move_base_goal.target_pose.pose.position.x=pose.pose.position.x;
+ this->move_base_goal.target_pose.pose.position.y=pose.pose.position.y;
+ this->move_base_goal.target_pose.pose.position.z=0.0;
+ this->move_base_goal.target_pose.pose.orientation=pose.pose.orientation;
+ }
+ return true;
+ }
+ else
+ {
+ ROS_WARN_STREAM("CNavModule: No transform found from " << this->current_pose.header.frame_id << " to " << this->goal_frame_id);
+ return false;
+ }
+ }
+ else
+ {
+ ROS_WARN_STREAM("CNavModule: Not receiving odometry messages");
+ return false;
+ }
+ }catch(tf2::TransformException &ex){
+ ROS_ERROR_STREAM("CNavModule: TF2 Exception: " << ex.what());
+ return false;
+ }
+
+ return false;
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::clear_costmaps_call(const ros::TimerEvent& event)
+ {
+ std_srvs::Empty clear_costmaps_req;
+
+ this->lock();
+ switch(this->clear_costmaps.call(clear_costmaps_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: Costmaps cleared sucessfully");
+ break;
+ case ACT_SRV_PENDING: ROS_WARN("CNavModule: Costmaps not yet cleared");
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to clear costmaps");
+ break;
+ }
+ this->unlock();
+ }
+
+ // common functions
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::set_goal_frame_id(std::string &frame_id)
+ {
+ this->lock();
+ this->goal_frame_id=frame_id;
+ this->unlock();
+ }
+
+ // costmap functions
+ template <class ModuleCfg>
+ bool CNavModule<ModuleCfg>::costmaps_clear(void)
+ {
+ std_srvs::Empty clear_costmaps_req;
+
+ this->lock();
+ if(!this->enable_auto_clear)
+ {
+ switch(this->clear_costmaps.call(clear_costmaps_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: Costmaps cleared sucessfully");
+ this->unlock();
+ return true;
+ case ACT_SRV_PENDING: ROS_WARN("CNavModule: Costmaps not yet cleared");
+ this->unlock();
+ return false;
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to clear costmaps");
+ this->unlock();
+ return false;
+ break;
+ }
+ }
+ else
+ {
+ this->unlock();
+ return false;
+ }
+
+ this->unlock();
+ return false;
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::costmaps_enable_auto_clear(double rate_hz)
+ {
+ this->lock();
+ if(!this->enable_auto_clear)
+ {
+ this->clear_costmaps_timer.setPeriod(ros::Duration(1.0/this->auto_clear_rate_hz));
+ this->clear_costmaps_timer.start();
+ this->enable_auto_clear=true;
+ }
+ this->unlock();
+ }
+
+ template <class ModuleCfg>
+ void CNavModule<ModuleCfg>::costmaps_disable_auto_clear(void)
+ {
+ this->lock();
+ if(this->enable_auto_clear)
+ {
+ this->clear_costmaps_timer.stop();
+ this->enable_auto_clear=false;
+ }
+ this->unlock();
+ }
+
+ template <class ModuleCfg>
+ bool CNavModule<ModuleCfg>::costamp_is_auto_clear_enabled(void)
+ {
+ return this->enable_auto_clear;
+ }
+
+ template <class ModuleCfg>
+ geometry_msgs::Pose CNavModule<ModuleCfg>::get_current_pose(void)
+ {
+ return this->current_pose.pose;
+ }
+
+ template <class ModuleCfg>
+ double CNavModule<ModuleCfg>::getGoalDistance(void)
+ {
+ this->lock();
+ if (!this->path_available)
+ {
+ ROS_WARN("CNavModule: Goal distance not calculed. No path received.");
+ this->unlock();
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ double dist = 0.0;
+ if (this->path_msg.poses.size() < 2)
+ {
+ ROS_WARN("CNavModule: Goal distance not calculed. Not enough points.");
+ this->unlock();
+ return std::numeric_limits<double>::quiet_NaN();
+ }
+ for (unsigned int i=0; i< this->path_msg.poses.size()-1; i++)
+ {
+ double dx = this->path_msg.poses[i].pose.position.x - this->path_msg.poses[i+1].pose.position.x;
+ double dy = this->path_msg.poses[i].pose.position.y - this->path_msg.poses[i+1].pose.position.y;
+ double d = std::sqrt(std::pow(dx,2) + std::pow(dy,2));
+ dist += d;
+ }
+ this->unlock();
+ return dist;
+ }
+
+ template<class ModuleCfg>
+ void CNavModule<ModuleCfg>::dynamic_reconfigure(ModuleCfg &config, const std::string &name)
+ {
+ std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr> params;
+ bool auto_clear_costmap;
+ boost::any value;
+
+ params=ModuleCfg::__getParamDescriptions__();
+
+ CModule<ModuleCfg>::dynamic_reconfigure(config,name+"_module");
+ this->move_base_action.dynamic_reconfigure(config,name+"_move_base");
+ this->make_plan_service.dynamic_reconfigure(config,name+"_make_plan");
+ this->clear_costmaps.dynamic_reconfigure(config,name+"_clear_costmap");
+
+ for(typename std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
+ {
+ if((*param)->name==(name+"_move_base_frame_id"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(std::string))
+ this->goal_frame_id=boost::any_cast<std::string &>(value);
+ }
+ else if((*param)->name==(name+"_move_base_cancel_prev"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(bool))
+ this->move_base_cancel_prev=boost::any_cast<bool &>(value);
+ }
+ else if((*param)->name==(name+"_odom_watchdog_time_s"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ this->odom_watchdog_time_s=boost::any_cast<double &>(value);
+ }
+ else if((*param)->name==(name+"_tf_timeout_time_s"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ this->tf_timeout_time_s=boost::any_cast<double &>(value);
+ }
+ else if((*param)->name==(name+"_clear_costmap_auto_clear_rate_hz"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(double))
+ this->auto_clear_rate_hz=boost::any_cast<double &>(value);
+ }
+ else if((*param)->name==(name+"_clear_costmap_enable_auto_clear"))
+ {
+ (*param)->getValue(config,value);
+ if(value.type()==typeid(bool))
+ auto_clear_costmap=boost::any_cast<bool &>(value);
+ }
+ }
+ if(auto_clear_costmap)
+ {
+ this->clear_costmaps_timer.setPeriod(ros::Duration(1.0/this->auto_clear_rate_hz));
+ this->clear_costmaps_timer.start();
+ this->enable_auto_clear=true;
+ }
+ else
+ {
+ this->clear_costmaps_timer.stop();
+ this->enable_auto_clear=false;
+ }
+ }
+
+ template <class ModuleCfg>
+ bool CNavModule<ModuleCfg>::make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance,nav_msgs::Path &plan)
+ {
+ nav_msgs::GetPlan get_plan_req;
+ tf2::Quaternion quat;
+
+ this->lock();
+ // start position
+ get_plan_req.request.start.header.frame_id=frame_id;
+ get_plan_req.request.start.header.stamp=ros::Time::now();
+ get_plan_req.request.start.pose.position.x=start_x;
+ get_plan_req.request.start.pose.position.y=start_y;
+ get_plan_req.request.start.pose.position.z=0.0;
+ quat.setRPY(0.0,0.0,start_yaw);
+ get_plan_req.request.start.pose.orientation=tf2::toMsg(quat);
+ // end position
+ get_plan_req.request.goal.header.frame_id=frame_id;
+ get_plan_req.request.goal.header.stamp=ros::Time::now();
+ get_plan_req.request.goal.pose.position.x=end_x;
+ get_plan_req.request.goal.pose.position.y=end_y;
+ get_plan_req.request.goal.pose.position.z=0.0;
+ quat.setRPY(0.0,0.0,end_yaw);
+ get_plan_req.request.goal.pose.orientation=tf2::toMsg(quat);
+ get_plan_req.request.tolerance=tolerance;
+ switch(this->make_plan_service.call(get_plan_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: New plan generated successfuilly");
+ this->unlock();
+ plan=get_plan_req.response.plan;
+ return true;
+ break;
+ case ACT_SRV_PENDING: ROS_WARN("CNavModule: Still waiting for new plan");
+ this->unlock();
+ return false;
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to get new plan");
+ this->unlock();
+ return false;
+ break;
+ default: this->unlock();
+ return false;
+ break;
+ }
+ }
+
+ /* parameters */
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::are_costmaps_shutdown(bool &shutdown)
+ {
+ if(this->move_base_reconf.get_parameter("shutdown_costmaps",shutdown))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::shutdown_costmaps(bool &shutdown)
+ {
+ this->move_base_reconf.set_parameter("shutdown_costmaps",shutdown);
+ return this->move_base_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::get_max_num_retries(unsigned int &retries)
+ {
+ if(this->move_base_reconf.get_parameter("max_planning_retries",retries))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::set_max_num_retries(unsigned int &retries)
+ {
+ this->move_base_reconf.set_parameter("max_planning_retries",retries);
+ return this->move_base_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::get_planner_frequency(double &freq)
+ {
+ if(this->move_base_reconf.get_parameter("planner_frequency",freq))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::set_planner_frequency(double &freq)
+ {
+ this->move_base_reconf.set_parameter("planner_frequency",freq);
+ return this->move_base_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::get_planner_patience(double &time)
+ {
+ if(this->move_base_reconf.get_parameter("planner_patience",time))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::set_planner_patience(double &time)
+ {
+ this->move_base_reconf.set_parameter("planner_patience",time);
+ return this->move_base_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::get_controller_frequency(double &freq)
+ {
+ if(this->move_base_reconf.get_parameter("controller_frequency",freq))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::set_controller_frequency(double &freq)
+ {
+ this->move_base_reconf.set_parameter("controller_frequency",freq);
+ return this->move_base_reconf.get_status();
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::get_controller_patience(double &time)
+ {
+ if(this->move_base_reconf.get_parameter("controller_patience",time))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template <class ModuleCfg>
+ dyn_reconf_status_t CNavModule<ModuleCfg>::set_controller_patience(double &time)
+ {
+ this->move_base_reconf.set_parameter("controller_patience",time);
+ return this->move_base_reconf.get_status();
+ }
+
+ /* costmaps */
+ template <class ModuleCfg>
+ const CNavCostmapModule<ModuleCfg> &CNavModule<ModuleCfg>::get_local_costmap(void) const
+ {
+ return this->local_costmap;
+ }
+
+ template <class ModuleCfg>
+ const CNavCostmapModule<ModuleCfg> &CNavModule<ModuleCfg>::get_global_costmap(void) const
+ {
+ return this->global_costmap;
+ }
+
+ template <class ModuleCfg>
+ CNavModule<ModuleCfg>::~CNavModule()
+ {
+ if(!this->move_base_action.is_finished())
+ {
+ this->move_base_action.cancel();
+ while(!this->move_base_action.is_finished());
+ }
+ }
+
+ #endif
diff --git a/include/adc_nav_module/nav_planner_module.h b/include/iri_adc_nav_module/nav_planner_module.h
similarity index 88%
rename from include/adc_nav_module/nav_planner_module.h
rename to include/iri_adc_nav_module/nav_planner_module.h
index f77b57798f073701fadf1e1d970da929ec2a62bd..3f625ff59480076223e52eee8109bf80d252fdf3 100644
--- a/include/adc_nav_module/nav_planner_module.h
+++ b/include/iri_adc_nav_module/nav_planner_module.h
@@ -12,7 +12,7 @@ template <class ModuleCfg>
class CNavPlannerModule
{
- private:
+ protected:
// dynamic reconfigure
/**
* \brief
@@ -35,7 +35,7 @@ class CNavPlannerModule
};
template <class ModuleCfg>
- CNavPlannerModule<ModuleCfg>::CNavPlannerModule(const std::string &name,const std::string &name_space) : planner_reconf(name,name_space)
+ CNavPlannerModule<ModuleCfg>::CNavPlannerModule(const std::string &name,const std::string &name_space) : planner_reconf("dyn_reconf",name_space+"/"+name)
{
}
diff --git a/include/iri_adc_nav_module/opendrive_gp_module.h b/include/iri_adc_nav_module/opendrive_gp_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..b161d827dedcdbca1e65e5b76bf3dc70a7efa19c
--- /dev/null
+++ b/include/iri_adc_nav_module/opendrive_gp_module.h
@@ -0,0 +1,264 @@
+#ifndef _OPENDRIVE_GP_MODULE_H
+#define _OPENDRIVE_GP_MODULE_H
+
+// IRI ROS headers
+#include <iri_ros_tools/module_service.h>
+#include <iri_adc_nav_module/nav_planner_module.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <iri_adc_msgs/get_opendrive_map.h>
+#include <iri_adc_msgs/get_opendrive_nodes.h>
+
+typedef enum {OD_GP_DIST_COST=0,OD_GP_TIME_COST=1} cost_type_t;
+
+/**
+ * \brief
+ *
+ */
+template <class ModuleCfg>
+class COpendriveGPModule : public CNavPlannerModule<ModuleCfg>
+{
+ private:
+ CModuleService<iri_adc_msgs::get_opendrive_map,ModuleCfg> get_map;
+ CModuleService<iri_adc_msgs::get_opendrive_nodes,ModuleCfg> get_nodes;
+ public:
+ /**
+ * \brief Constructor
+ *
+ */
+ COpendriveGPModule(const std::string &name,const std::string &name_space=std::string(""));
+ /* parameter functions */
+ dyn_reconf_status_t is_multi_hypothesis(bool &enable);
+ dyn_reconf_status_t multy_hypothesis(bool enable);
+ dyn_reconf_status_t get_cost_type(cost_type_t &type);
+ dyn_reconf_status_t set_cost_type(cost_type_t type);
+ dyn_reconf_status_t get_opendrive_map(std::string &map_file);
+ dyn_reconf_status_t set_opendrive_map(const std::string &map_file);
+ dyn_reconf_status_t get_opendrive_frame(std::string &frame);
+ dyn_reconf_status_t set_opendrive_frame(const std::string &frame);
+ dyn_reconf_status_t get_distance_tolerance(double &tolerance);
+ dyn_reconf_status_t set_distance_tolerance(double tolerance);
+ dyn_reconf_status_t get_angle_tolerance(double &tolerance);
+ dyn_reconf_status_t set_angle_tolerance(double tolerance);
+ dyn_reconf_status_t get_resolution(double &resolution);
+ dyn_reconf_status_t set_resolution(double resolution);
+ dyn_reconf_status_t get_scale_factor(double &scale);
+ dyn_reconf_status_t set_scale_factor(double scale);
+ dyn_reconf_status_t get_min_road_length(double &length);
+ dyn_reconf_status_t set_min_raod_length(double length);
+ /**
+ * \brief Destructor
+ *
+ */
+ bool get_opendrive_map(nav_msgs::OccupancyGrid &map);
+ /**
+ * \brief
+ *
+ */
+ bool get_opendrive_nodes(std::vector<unsigned int> &nodes);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~COpendriveGPModule();
+ };
+
+ template<class ModuleCfg>
+ COpendriveGPModule<ModuleCfg>::COpendriveGPModule(const std::string &name,const std::string &name_space) : CNavPlannerModule<ModuleCfg>(name,name_space),
+ get_map("get_map",name_space+"/"+name),
+ get_nodes("get_nodes",name_space+"/"+name)
+ {
+
+ }
+
+ /* parameter functions */
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::is_multi_hypothesis(bool &enable)
+ {
+ if(this->planner_reconf.get_parameter("multi_hyp",enable))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::multy_hypothesis(bool enable)
+ {
+ this->planner_reconf.set_parameter("multi_hyp",enable);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_cost_type(cost_type_t &type)
+ {
+ if(this->planner_reconf.get_parameter("cost_type",(int)type))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_cost_type(cost_type_t type)
+ {
+ this->planner_reconf.set_parameter("cost_type",(int)type);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_opendrive_map(std::string &map_file)
+ {
+ if(this->planner_reconf.get_parameter("opendrive_file",map_file))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_opendrive_map(const std::string &map_file)
+ {
+ this->planner_reconf.set_parameter("opendrive_file",map_file);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_opendrive_frame(std::string &frame)
+ {
+ if(this->planner_reconf.get_parameter("opendrive_frame",frame))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_opendrive_frame(const std::string &frame)
+ {
+ this->planner_reconf.set_parameter("opendrive_frame",frame);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_distance_tolerance(double &tolerance)
+ {
+ if(this->planner_reconf.get_parameter("dist_tol",tolerance))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_distance_tolerance(double tolerance)
+ {
+ this->planner_reconf.set_parameter("dist_tol",tolerance);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_angle_tolerance(double &tolerance)
+ {
+ if(this->planner_reconf.get_parameter("angle_tol",tolerance))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_angle_tolerance(double tolerance)
+ {
+ this->planner_reconf.set_parameter("angle_tol",tolerance);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_resolution(double &resolution)
+ {
+ if(this->planner_reconf.get_parameter("resolution",resolution))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_resolution(double resolution)
+ {
+ this->planner_reconf.set_parameter("resolution",resolution);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_scale_factor(double &scale)
+ {
+ if(this->planner_reconf.get_parameter("scale_factor",scale))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_scale_factor(double scale)
+ {
+ this->planner_reconf.set_parameter("scale_factor",scale);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::get_min_road_length(double &length)
+ {
+ if(this->planner_reconf.get_parameter("min_road_length",length))
+ return DYN_RECONF_SUCCESSFULL;
+ else
+ return DYN_RECONF_NO_SUCH_PARAM;
+ }
+
+ template<class ModuleCfg>
+ dyn_reconf_status_t COpendriveGPModule<ModuleCfg>::set_min_raod_length(double length)
+ {
+ this->planner_reconf.set_parameter("min_road_length",length);
+ return this->planner_reconf.get_status();
+ }
+
+ template<class ModuleCfg>
+ bool COpendriveGPModule<ModuleCfg>::get_opendrive_map(nav_msgs::OccupancyGrid &map)
+ {
+ iri_adc_msgs::get_opendrive_map get_map_req;
+
+ switch(this->get_map.call(get_map_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("COpendriveGPModule: Opendrive map returned");
+ map=get_map_req.response.opendrive_map;
+ return true;
+ case ACT_SRV_PENDING: ROS_WARN("COpendriveGPModule: Opendrive map not available yet");
+ return false;
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("COpendriveGPModule: Impossible to get opendrive map");
+ return false;
+ break;
+ }
+ return false;
+ }
+
+ template<class ModuleCfg>
+ bool COpendriveGPModule<ModuleCfg>::get_opendrive_nodes(std::vector<unsigned int> &nodes)
+ {
+ iri_adc_msgs::get_opendrive_nodes get_nodes_req;
+
+ switch(this->get_nodes.call(get_nodes_req))
+ {
+ case ACT_SRV_SUCCESS: ROS_INFO("COpendriveGPModule: Current plan opendrive nodes returned");
+ nodes=get_nodes_req.response.opendrive_nodes.nodes;
+ return true;
+ case ACT_SRV_PENDING: ROS_WARN("COpendriveGPModule: current plan opendrive nodes not available yet");
+ return false;
+ break;
+ case ACT_SRV_FAIL: ROS_ERROR("COpendriveGPModule: Impossible to get current plan opendrive nodes");
+ return false;
+ break;
+ }
+ return false;
+ }
+
+ template<class ModuleCfg>
+ COpendriveGPModule<ModuleCfg>::~COpendriveGPModule()
+ {
+
+ }
+
+ #endif
diff --git a/launch/tiago_nav_bt_client.launch b/launch/adc_nav_bt_client.launch
similarity index 100%
rename from launch/tiago_nav_bt_client.launch
rename to launch/adc_nav_bt_client.launch
diff --git a/launch/adc_nav_client.launch b/launch/adc_nav_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..1e196dc37799c463aaa9e934c421d40081c8ce97
--- /dev/null
+++ b/launch/adc_nav_client.launch
@@ -0,0 +1,59 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="node_name" default="nav_client" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find iri_adc_nav_module)/config/adc_nav_module_default.yaml" />
+ <arg name="move_base_action" default="/adc_car/move_base"/>
+ <arg name="odom_topic" default="/adc_car/local_odom_combined"/>
+ <arg name="path_topic" default="/adc_car/move_base/OpendriveGlobalPlanner/plan"/>
+ <arg name="clear_costmaps_service" default="/adc_car/move_base/clear_costmaps"/>
+ <arg name="make_plan_service" default="/adc_car/move_base/make_plan"/>
+ <arg name="move_base_dyn_reconf" default="/adc_car/move_base/set_parameters"/>
+ <arg name="local_costmap_dyn_reconf" default="/adc_car/move_base/local_costmap/set_parameters"/>
+ <arg name="local_planner_dyn_reconf" default="/adc_car/move_base/AckermannPlannerROS/set_parameters"/>
+ <arg name="global_costmap_dyn_reconf" default="/adc_car/move_base/global_costmap/set_parameters"/>
+ <arg name="global_planner_dyn_reconf" default="/adc_car/move_base/OpendriveGlobalPlanner/set_parameters"/>
+ <arg name="get_opendrive_map_service" default="/adc_car/move_base/OpendriveGlobalPlanner/get_opendrive_map"/>
+ <arg name="get_opendrive_nodes_service" default="/adc_car/move_base/OpendriveGlobalPlanner/get_opendrive_nodes"/>
+
+ <!-- launch the play motion client node -->
+ <node name="$(arg node_name)"
+ pkg="iri_adc_nav_module"
+ type="adc_nav_client"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~/nav_module/move_base"
+ to="$(arg move_base_action)"/>
+ <remap from="~/nav_module/odom"
+ to="$(arg odom_topic)"/>
+ <remap from="~/nav_module/path"
+ to="$(arg path_topic)"/>
+ <remap from="~/nav_module/clear_costmaps"
+ to="$(arg clear_costmaps_service)"/>
+ <remap from="~/nav_module/make_plan"
+ to="$(arg make_plan_service)"/>
+ <remap from="~/nav_module/move_base_reconf"
+ to="$(arg move_base_dyn_reconf)"/>
+ <remap from="~/nav_module/local_costmap/dyn_reconf"
+ to="$(arg local_costmap_dyn_reconf)"/>
+ <remap from="~/nav_module/local_planner/dyn_reconf"
+ to="$(arg local_planner_dyn_reconf)"/>
+ <remap from="~/nav_module/global_costmap/dyn_reconf"
+ to="$(arg global_costmap_dyn_reconf)"/>
+ <remap from="~/nav_module/global_planner/dyn_reconf"
+ to="$(arg global_planner_dyn_reconf)"/>
+ <remap from="~/nav_module/global_planner/get_map"
+ to="$(arg get_opendrive_map_service)"/>
+ <remap from="~/nav_module/global_planner/get_nodes"
+ to="$(arg get_opendrive_nodes_service)"/>
+
+ <rosparam file="$(arg config_file)" command="load" ns="nav_module" />
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/launch/ivo_nav_bt_client.launch b/launch/ivo_nav_bt_client.launch
deleted file mode 100644
index 480be466d531081e42475e78cfb17fe5c12ffda2..0000000000000000000000000000000000000000
--- a/launch/ivo_nav_bt_client.launch
+++ /dev/null
@@ -1,41 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_nav_bt_client_sim.launch b/launch/ivo_nav_bt_client_sim.launch
deleted file mode 100644
index ba18392e8ae0439ace8cb0b5969e49f4dfef020a..0000000000000000000000000000000000000000
--- a/launch/ivo_nav_bt_client_sim.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- </include>
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_nav_client.launch b/launch/ivo_nav_client.launch
deleted file mode 100644
index c06285f5fe0107a605b04241be056ba5944ddcf5..0000000000000000000000000000000000000000
--- a/launch/ivo_nav_client.launch
+++ /dev/null
@@ -1,34 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_nav_client_sim.launch b/launch/ivo_nav_client_sim.launch
deleted file mode 100644
index 40da268f0f318c66112d6b0c071d82968576dcc4..0000000000000000000000000000000000000000
--- a/launch/ivo_nav_client_sim.launch
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- </include>
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_nav_bt_client_sim.launch b/launch/tiago_nav_bt_client_sim.launch
deleted file mode 100644
index dae4375d1938780ff85def764a9be11eef8a2430..0000000000000000000000000000000000000000
--- a/launch/tiago_nav_bt_client_sim.launch
+++ /dev/null
@@ -1,46 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- </include>
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_nav_client.launch b/launch/tiago_nav_client.launch
deleted file mode 100644
index df19caa174bd4eaa3d2635f345608ab5713f945a..0000000000000000000000000000000000000000
--- a/launch/tiago_nav_client.launch
+++ /dev/null
@@ -1,50 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_nav_module"
- type="tiago_nav_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/nav_module/move_base"
- to="$(arg move_base_action)"/>
- <remap from="~/nav_module/move_poi"
- to="$(arg move_poi_action)"/>
- <remap from="~/nav_module/move_waypoint"
- to="$(arg move_waypoint_action)"/>
- <remap from="~/nav_module/odom"
- to="$(arg odom_topic)"/>
- <remap from="~/nav_module/path"
- to="$(arg path_topic)"/>
- <remap from="~/nav_module/clear_costmaps"
- to="$(arg clear_costmaps_service)"/>
- <remap from="~/nav_module/change_map"
- to="$(arg change_map_service)"/>
- <remap from="~/nav_module/set_op_mode"
- to="$(arg set_op_mode_service)"/>
- <remap from="~/nav_module/move_base_reconf"
- to="$(arg move_base_dyn_reconf)"/>
-
- <rosparam file="$(arg config_file)" command="load" ns="nav_module" />
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/tiago_nav_client_sim.launch b/launch/tiago_nav_client_sim.launch
deleted file mode 100644
index 40da268f0f318c66112d6b0c071d82968576dcc4..0000000000000000000000000000000000000000
--- a/launch/tiago_nav_client_sim.launch
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="nav_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
- <arg name="move_base_action" default="/move_base"/>
- <arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
- <arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
- <arg name="odom_topic" default="/mobile_base_controller/odom"/>
- <arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
- <arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
- <arg name="change_map_service" default="/pal_map_manager/change_map"/>
- <arg name="set_op_mode_service" default="/pal_navigation_sm"/>
- <arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
-
- <include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- </include>
-
- <include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_base_action" value="$(arg move_base_action)"/>
- <arg name="move_poi_action" value="$(arg move_poi_action)"/>
- <arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
- <arg name="odom_topic" value="$(arg odom_topic)"/>
- <arg name="path_topic" value="$(arg path_topic)"/>
- <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
- <arg name="change_map_service" value="$(arg change_map_service)"/>
- <arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
- <arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
- </include>
-
-</launch>
-
diff --git a/package.xml b/package.xml
index eabd987c697ca1b63bcc64513ec5f4446b09ab3f..16998b76ff4d6f66a768a1494d10ef4512a1a8a4 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package>
- <name>adc_nav_module</name>
+ <name>iri_adc_nav_module</name>
<version>0.0.0</version>
- <description>Module adc_nav_module package</description>
+ <description>Module iri_adc_nav_module package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
@@ -51,6 +51,7 @@
<build_depend>iri_base_algorithm</build_depend>
<build_depend>iri_base_bt_client</build_depend>
<build_depend>iri_behaviortree</build_depend>
+ <build_depend>iri_adc_msgs</build_depend>
<!-- new dependencies -->
<!--<build_depend>new build dependency</build_depend>-->
<run_depend>iri_ros_tools</run_depend>
@@ -64,6 +65,7 @@
<run_depend>iri_base_algorithm</run_depend>
<run_depend>iri_base_bt_client</run_depend>
<run_depend>iri_behaviortree</run_depend>
+ <run_depend>iri_adc_msgs</run_depend>
<!-- new dependencies -->
<!--<run_depend>new run dependency</run_depend>-->
diff --git a/setup.py b/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..ff212abd8acded77068278427b8017891cd6398c
--- /dev/null
+++ b/setup.py
@@ -0,0 +1,9 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+d = generate_distutils_setup(
+ packages=['iri_adc_nav_module'],
+ package_dir={'': 'src'}
+)
+setup(**d)
+
diff --git a/src/adc_nav_client_alg_node.cpp b/src/adc_nav_client_alg_node.cpp
index 3aee72d1c923689672fa8c97e09ceccc10da9dbb..eab6a41342f5d7a463b543179b3b7385a7ef9f00 100644
--- a/src/adc_nav_client_alg_node.cpp
+++ b/src/adc_nav_client_alg_node.cpp
@@ -1,5 +1,7 @@
#include "adc_nav_client_alg_node.h"
-#include <adc_nav_module/adc_nav_module.h>
+#include <iri_adc_nav_module/adc_nav_module.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/transform_listener.h>
ADCNavClientAlgNode::ADCNavClientAlgNode(void) :
algorithm_base::IriBaseAlgorithm<ADCNavClientAlgorithm>(),
@@ -48,19 +50,19 @@ void ADCNavClientAlgNode::node_config_update(Config &config, uint32_t level)
this->alg_.lock();
if(config.start_pose_nav)
{
- this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.set_goal_frame_id(config.mb_frame_id);
this->nav.go_to_pose(config.mb_x_pos,config.mb_y_pos,config.mb_yaw,config.mb_xy_tol,config.mb_yaw_tol);
config.start_pose_nav=false;
}
else if(config.start_position_nav)
{
- this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.set_goal_frame_id(config.mb_frame_id);
this->nav.go_to_position(config.mb_x_pos,config.mb_y_pos,config.mb_xy_tol);
config.start_position_nav=false;
}
else if(config.start_orientation_nav)
{
- this->nav.set_goal_frame(config.mb_frame_id);
+ this->nav.set_goal_frame_id(config.mb_frame_id);
this->nav.go_to_orientation(config.mb_yaw,config.mb_yaw_tol);
config.start_orientation_nav=false;
}
@@ -84,12 +86,45 @@ void ADCNavClientAlgNode::node_config_update(Config &config, uint32_t level)
this->nav.costmaps_disable_auto_clear();
config.cm_disable_auto_clear=false;
}
- if (config.print_goal_distance)
+ else if (config.print_goal_distance)
{
double dist = this->nav.getGoalDistance();
ROS_INFO_STREAM("Goal distance = " << dist);
config.print_goal_distance = false;
}
+ else if(config.get_plan)
+ {
+ nav_msgs::Path plan;
+ double roll, pitch, yaw;
+ geometry_msgs::Pose current_pose;
+
+ current_pose=this->nav.get_current_pose();
+ tf2::Matrix3x3 m(tf2::Quaternion(current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w));
+ m.getRPY(roll, pitch, yaw);
+ this->nav.make_plan(current_pose.position.x,current_pose.position.x,yaw,config.mb_x_pos,config.mb_y_pos,config.mb_yaw,config.mb_frame_id,config.mb_xy_tol,plan);
+ for(unsigned int i=0;i<plan.poses.size();i++)
+ std::cout << plan.poses[i].pose.position.x << "," << plan.poses[i].pose.position.y << std::endl;
+ config.get_plan=false;
+ }
+ else if(config.get_opendrive_map)
+ {
+ nav_msgs::OccupancyGrid map;
+
+ this->nav.get_opendrive_map(map);
+ config.get_opendrive_map=false;
+ }
+ else if(config.get_opendrive_nodes)
+ {
+ std::vector<unsigned int> nodes;
+
+ if(this->nav.get_opendrive_nodes(nodes))
+ {
+ for(unsigned int i=0;i<nodes.size();i++)
+ std::cout << nodes[i] << ",";
+ std::cout << std::endl;
+ }
+ config.get_opendrive_nodes=false;
+ }
this->alg_.unlock();
}
diff --git a/src/adc_nav_module.cpp b/src/adc_nav_module.cpp
index 2183190735764cbe25608904a1c397e4cb1df77a..2450d7ac58832f05cadd47448968ccd1b46f7347 100644
--- a/src/adc_nav_module.cpp
+++ b/src/adc_nav_module.cpp
@@ -1,9 +1,9 @@
-#include <adc_nav_module/adc_nav_module.h>
+#include <iri_adc_nav_module/adc_nav_module.h>
#include <tf/transform_datatypes.h>
CADCNavModule::CADCNavModule(const std::string &name,const std::string &name_space) : CNavModule(name,name_space),
- CAckermannLPModule(name,name_space),
- COpendriveGPModule(name,name_space)
+ CAckermannLPModule("local_planner",this->module_nh.getNamespace()),
+ COpendriveGPModule("global_planner",this->module_nh.getNamespace())
{
this->start_operation();
@@ -49,7 +49,7 @@ void CADCNavModule::state_machine(void)
switch(this->start_action(this->goal_x,this->goal_y,this->goal_yaw))
{
case ACT_SRV_SUCCESS: this->state=ADC_NAV_MODULE_WAIT;
- ROS_DEBUG("CADCNavModule : goal start successfull");
+ ROS_INFO("CADCNavModule : goal start successfull");
break;
case ACT_SRV_PENDING: this->state=ADC_NAV_MODULE_START;
ROS_WARN("CADCNavModule : goal start pending");
@@ -107,6 +107,14 @@ void CADCNavModule::state_machine(void)
}
}
+void CADCNavModule::reconfigure_callback(iri_adc_nav_module::ADCNavModuleConfig &config, uint32_t level)
+{
+ this->lock();
+ CNavModule::dynamic_reconfigure(config,"nav");
+ this->config=config;
+ this->unlock();
+}
+
// basic navigation functions
bool CADCNavModule::go_to_orientation(double yaw,double heading_tol)
{
diff --git a/src/iri_adc_nav_module/__init__.py b/src/iri_adc_nav_module/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/iri_adc_nav_module/dyn_params.py b/src/iri_adc_nav_module/dyn_params.py
new file mode 100644
index 0000000000000000000000000000000000000000..c396cdb5b8b0c00cccd073d0e72fe7c32c8bce7d
--- /dev/null
+++ b/src/iri_adc_nav_module/dyn_params.py
@@ -0,0 +1,29 @@
+# submodule config parameters
+from dynamic_reconfigure.parameter_generator_catkin import *
+from iri_ros_tools.dyn_params import add_module_service_params,add_module_action_params,add_module_params
+
+def add_nav_module_params(gen,name):
+
+ new_group = gen.add_group(name)
+
+ odom = new_group.add_group(name+"_Odometry")
+ tf = new_group.add_group(name+"_TF")
+
+ add_module_params(new_group,name+"_module")
+
+ move_base_action=add_module_action_params(new_group,name+"_move_base")
+ move_base_action.add(name+"_move_base_frame_id",str_t, 0, "Reference frame of the position goals","map")
+ move_base_action.add(name+"_move_base_cancel_prev", bool_t, 0, "Cancel previous action", True)
+
+ odom.add(name+"_odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50)
+
+ tf.add(name+"_tf_timeout_time_s", double_t, 0, "Maximum time to wait for transform",5, 0.01, 50)
+
+ costmap=add_module_service_params(new_group,name+"_clear_costmap")
+ costmap.add(name+"_clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False)
+ costmap.add(name+"_clear_costmap_auto_clear_rate_hz",double_t, 0, "Clear costmaps period", 0.1, 0.01, 1.0)
+
+ make_plan=add_module_service_params(new_group,name+"_make_plan")
+
+ return new_group
+
diff --git a/src/iri_adc_nav_module/dyn_params.pyc b/src/iri_adc_nav_module/dyn_params.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3c8e0755e31367a17eb059cb19931ea0b1c836f3
Binary files /dev/null and b/src/iri_adc_nav_module/dyn_params.pyc differ
diff --git a/src/nav_module.cpp b/src/nav_module.cpp
deleted file mode 100644
index 41edeb90049015c69aee87c9ab95979797881c20..0000000000000000000000000000000000000000
--- a/src/nav_module.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-#include <adc_nav_module/nav_module.h>
-
-CNavModule::CNavModule(const std::string &name,const std::string &name_space) : CModule(name,name_space),
- tf2_listener(tf2_buffer),
- move_base_action("move_base",this->module_nh.getNamespace()),
- move_base_reconf("move_base_reconf",this->module_nh.getNamespace()),
- make_plan_service("make_plan",this->module_nh.getNamespace()),
- clear_costmaps("clear_costmaps",this->module_nh.getNamespace()),
- local_costmap("local_costmap",this->module_nh.getNamespace()),
- global_costmap("global_costmap",this->module_nh.getNamespace())
-{
- //Variables
- this->path_available = false;
-
- // initialize odometry subscriber
- if(!this->module_nh.getParam("odom_watchdog_time_s", this->config.odom_watchdog_time_s))
- {
- ROS_WARN("CNavModule::CNavModule: param 'odom_watchdog_time_s' not found. Configuring wathcdog with 1.0 sec");
- this->odom_watchdog.reset(ros::Duration(1.0));
- }
- else
- this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
-
- // this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
- this->odom_subscriber = this->module_nh.subscribe("odom",1,&CNavModule::odom_callback,this);
-
- // initialize path subscriber
- this->path_subscriber = this->module_nh.subscribe("path", 1, &CNavModule::path_callback, this);
-
- // costmaps
- if(!this->module_nh.getParam("clear_costmap_auto_clear_rate_hz", this->config.clear_costmap_auto_clear_rate_hz))
- {
- ROS_WARN("CNavModule::CNavModule: param 'clear_costmap_auto_clear_rate_hz' not found. Configuring wathcdog with 0.1 Hz");
- this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/0.1),&CNavModule::clear_costmaps_call,this);
- }
- else
- this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CNavModule::clear_costmaps_call,this);
- // this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CNavModule::clear_costmaps_call,this);
- this->clear_costmaps_timer.stop();
- this->enable_auto_clear=false;
-}
-
-std::string CNavModule::get_pose_frame_id(void)
-{
- return this->current_pose.header.frame_id;
-}
-
-std::string CNavModule::get_goal_frame_id(void)
-{
- return this->goal_frame_id;
-}
-
-act_srv_status CNavModule::start_action(double x,double y,double yaw)
-{
- tf2::Quaternion quat;
- act_srv_status status;
-
- this->lock();
- this->move_base_goal.target_pose.header.stamp=ros::Time::now();
- this->move_base_goal.target_pose.header.frame_id=this->goal_frame_id;
- this->move_base_goal.target_pose.pose.position.x=x;
- this->move_base_goal.target_pose.pose.position.y=y;
- this->move_base_goal.target_pose.pose.position.z=0.0;
- quat.setRPY(0.0,0.0,yaw);
- this->move_base_goal.target_pose.pose.orientation=tf2::toMsg(quat);
- this->path_available=false;
- if(this->config.move_base_cancel_prev && !this->move_base_action.is_finished())
- this->move_base_action.cancel();
- status=this->move_base_action.make_request(this->move_base_goal);
- this->unlock();
-
- return status;
-}
-
-void CNavModule::cancel_action(void)
-{
- this->move_base_action.cancel();
-}
-
-action_status CNavModule::get_action_status(void)
-{
- action_status status;
-
- this->lock();
- status=this->move_base_action.get_state();
- if(status!=ACTION_IDLE && status!=ACTION_RUNNING)
- this->move_base_action.stop_timeout();
- if(status==ACTION_TIMEOUT || status==ACTION_FB_WATCHDOG)
- this->move_base_action.cancel();
- this->unlock();
-
- return status;
-}
-
-void CNavModule::reconfigure_callback(adc_nav_module::NavModuleConfig &config, uint32_t level)
-{
- ROS_DEBUG("CNavModule : reconfigure callback");
- this->lock();
- this->config=config;
- /* set the module rate */
- this->dynamic_reconfigure(config,"nav_module");
- /* move base action parameters */
- this->move_base_action.dynamic_reconfigure(config,"move_base");
- this->goal_frame_id=config.move_base_frame_id;
- // costmaps
- this->clear_costmaps.dynamic_reconfigure(config,"clear_costmap");
- if(config.clear_costmap_enable_auto_clear)
- {
- this->clear_costmaps_timer.setPeriod(ros::Duration(1.0/config.clear_costmap_auto_clear_rate_hz));
- this->clear_costmaps_timer.start();
- this->enable_auto_clear=true;
- }
- else
- {
- this->clear_costmaps_timer.stop();
- this->enable_auto_clear=false;
- }
- this->unlock();
-}
-
-void CNavModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
-{
- ROS_DEBUG("CNavModule: odometry callback");
-
- this->lock();
- // reset the odometry callback
- this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
- // save the current position of the robot
- this->current_pose.pose=msg->pose.pose;
- this->current_pose.header=msg->header;
- this->unlock();
-}
-
-void CNavModule::path_callback(const nav_msgs::Path::ConstPtr& msg)
-{
- ROS_DEBUG("CNavModule: path callback");
-
- this->lock();
- this->path_msg = *msg;
- this->path_available = true;
- this->unlock();
-}
-
-bool CNavModule::transform_current_pose(void)
-{
- geometry_msgs::TransformStamped transform;
- geometry_msgs::PoseStamped pose;
- bool tf2_exists;
-
- try{
- tf2_exists=this->tf2_buffer.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp,ros::Duration(this->config.tf_timeout_time_s));
- transform = this->tf2_buffer.lookupTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp);
- if(tf2_exists)
- {
- tf2::doTransform(this->current_pose,pose,transform);
- if(!this->odom_watchdog.is_active())
- {
- this->move_base_goal.target_pose.pose.position.x=pose.pose.position.x;
- this->move_base_goal.target_pose.pose.position.y=pose.pose.position.y;
- this->move_base_goal.target_pose.pose.position.z=0.0;
- this->move_base_goal.target_pose.pose.orientation=pose.pose.orientation;
- }
- return true;
- }
- else
- {
- ROS_WARN_STREAM("No transform found from " << this->current_pose.header.frame_id << " to " << this->goal_frame_id);
- return false;
- }
- }catch(tf2::TransformException &ex){
- ROS_ERROR_STREAM("TF2 Exception: " << ex.what());
- return false;
- }
-
- return false;
-}
-
-void CNavModule::clear_costmaps_call(const ros::TimerEvent& event)
-{
- std_srvs::Empty clear_costmaps_req;
-
- this->lock();
- switch(this->clear_costmaps.call(clear_costmaps_req))
- {
- case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: Costmaps cleared sucessfully");
- break;
- case ACT_SRV_PENDING: ROS_WARN("CNavModule: Costmaps not yet cleared");
- break;
- case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to clear costmaps");
- break;
- }
- this->unlock();
-}
-
-// common functions
-void CNavModule::set_goal_frame(std::string &frame_id)
-{
- this->lock();
- this->goal_frame_id=frame_id;
- this->unlock();
-}
-
-// costmap functions
-bool CNavModule::costmaps_clear(void)
-{
- std_srvs::Empty clear_costmaps_req;
-
- this->lock();
- if(!this->enable_auto_clear)
- {
- switch(this->clear_costmaps.call(clear_costmaps_req))
- {
- case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: Costmaps cleared sucessfully");
- this->unlock();
- return true;
- case ACT_SRV_PENDING: ROS_WARN("CNavModule: Costmaps not yet cleared");
- this->unlock();
- return false;
- break;
- case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to clear costmaps");
- this->unlock();
- return false;
- break;
- }
- }
- else
- {
- this->unlock();
- return false;
- }
-
- this->unlock();
- return false;
-}
-
-void CNavModule::costmaps_enable_auto_clear(double rate_hz)
-{
- this->lock();
- if(!this->enable_auto_clear)
- {
- this->clear_costmaps_timer.setPeriod(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz));
- this->clear_costmaps_timer.start();
- this->enable_auto_clear=true;
- }
- this->unlock();
-}
-
-void CNavModule::costmaps_disable_auto_clear(void)
-{
- this->lock();
- if(this->enable_auto_clear)
- {
- this->clear_costmaps_timer.stop();
- this->enable_auto_clear=false;
- }
- this->unlock();
-}
-
-bool CNavModule::costamp_is_auto_clear_enabled(void)
-{
- return this->enable_auto_clear;
-}
-
-geometry_msgs::Pose CNavModule::getCurrentPose(void)
-{
- return this->current_pose.pose;
-}
-
-double CNavModule::getGoalDistance(void)
-{
- this->lock();
- if (!this->path_available)
- {
- ROS_WARN("CNavModule::getGoalDistance -> Goal distance not calculed. No path received.");
- this->unlock();
- return std::numeric_limits<double>::quiet_NaN();
- }
- double dist = 0.0;
- if (this->path_msg.poses.size() < 2)
- {
- ROS_WARN("CNavModule::getGoalDistance -> Goal distance not calculed. Not enough points.");
- this->unlock();
- return std::numeric_limits<double>::quiet_NaN();
- }
- for (unsigned int i=0; i< this->path_msg.poses.size()-1; i++)
- {
- double dx = this->path_msg.poses[i].pose.position.x - this->path_msg.poses[i+1].pose.position.x;
- double dy = this->path_msg.poses[i].pose.position.y - this->path_msg.poses[i+1].pose.position.y;
- double d = std::sqrt(std::pow(dx,2) + std::pow(dy,2));
- dist += d;
- }
- this->unlock();
- return dist;
-}
-
-bool CNavModule::make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,std::string &frame_id,double tolerance)
-{
- nav_msgs::GetPlan get_plan_req;
- tf2::Quaternion quat;
-
- this->lock();
- // start position
- get_plan_req.request.start.header.frame_id=frame_id;
- get_plan_req.request.start.header.stamp=ros::Time::now();
- get_plan_req.request.start.pose.position.x=start_x;
- get_plan_req.request.start.pose.position.y=start_y;
- get_plan_req.request.start.pose.position.z=0.0;
- quat.setRPY(0.0,0.0,start_yaw);
- get_plan_req.request.start.pose.orientation=tf2::toMsg(quat);
- // end position
- get_plan_req.request.goal.header.frame_id=frame_id;
- get_plan_req.request.goal.header.stamp=ros::Time::now();
- get_plan_req.request.goal.pose.position.x=end_x;
- get_plan_req.request.goal.pose.position.y=end_y;
- get_plan_req.request.goal.pose.position.z=0.0;
- quat.setRPY(0.0,0.0,end_yaw);
- get_plan_req.request.goal.pose.orientation=tf2::toMsg(quat);
- get_plan_req.request.tolerance=tolerance;
- switch(this->make_plan_service.call(get_plan_req))
- {
- case ACT_SRV_SUCCESS: ROS_INFO("CNavModule: New plan generated successfuilly");
- this->unlock();
- return true;
- break;
- case ACT_SRV_PENDING: ROS_WARN("CNavModule: Still waiting for new plan");
- this->unlock();
- return false;
- break;
- case ACT_SRV_FAIL: ROS_ERROR("CNavModule: Impossible to get new plan");
- this->unlock();
- return false;
- break;
- default: this->unlock();
- break;
- }
-}
-
-/* parameters */
-dyn_reconf_status_t CNavModule::are_costmaps_shutdown(bool &shutdown)
-{
- this->move_base_reconf.set_parameter("shutdown_costmaps",shutdown);
- return this->move_base_reconf.get_status();
-}
-
-dyn_reconf_status_t CNavModule::shutdown_costmaps(bool &shutdown)
-{
- this->move_base_reconf.set_parameter("shutdown_costmaps",shutdown);
- return this->move_base_reconf.get_status();
-}
-
-dyn_reconf_status_t CNavModule::get_max_num_retries(unsigned int &retries)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::set_max_num_retries(unsigned int &retries)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::get_planner_frequency(double &freq)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::set_planner_frequency(double &freq)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::get_planner_patience(double &time)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::set_planner_patience(double &time)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::get_controller_frequency(double &freq)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::set_controller_frequency(double &freq)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::get_controller_patience(double &time)
-{
-
-}
-
-dyn_reconf_status_t CNavModule::set_controller_patience(double &time)
-{
-
-}
-
-/* costmaps */
-const CNavCostmapModule<adc_nav_module::NavModuleConfig> &CNavModule::get_local_costmap(void) const
-{
- return this->local_costmap;
-}
-
-const CNavCostmapModule<adc_nav_module::NavModuleConfig> &CNavModule::get_global_costmap(void) const
-{
- return this->global_costmap;
-}
-
-CNavModule::~CNavModule()
-{
- if(!this->move_base_action.is_finished())
- {
- this->move_base_action.cancel();
- while(!this->move_base_action.is_finished());
- }
-}
diff --git a/src/nav_planner_module.cpp b/src/nav_planner_module.cpp
deleted file mode 100644
index 16cee64ce7d855e91fa9b9d060d1ebe07d37d659..0000000000000000000000000000000000000000
--- a/src/nav_planner_module.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#include <adc_nav_module/nav_planner_module.h>
-
-template <class ModuleCfg>
-CNavPlannerModule<ModuleCfg>::CNavPlannerModule(const std::string &name,const std::string &name_space)
-{
-
-}
-
-template <class ModuleCfg>
-CNavPlannerModule<ModuleCfg>::~CNavPlannerModule()
-{
-
-}
-