From 7a85e28ea401a7211c7ceebace4f7fb9064f79cf Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 29 Oct 2021 19:12:53 +0200
Subject: [PATCH] Initial implementation of a generic navigation module.
Implemented for the ADC navigation module.
---
CMakeLists.txt | 22 +-
cfg/{ADCNavModule.cfg => NavModule.cfg} | 2 +-
include/adc_nav_module/ackermann_lp_module.h | 290 ++++++++++
include/adc_nav_module/adc_nav_module.h | 307 +----------
include/adc_nav_module/nav_costmap_module.h | 21 +-
include/adc_nav_module/nav_module.h | 296 ++--------
include/adc_nav_module/nav_planner_module.h | 13 +-
include/adc_nav_module/opendrive_gp_module.h | 50 ++
package.xml | 4 +-
src/ackermann_lp_module.cpp | 326 +++++++++++
src/adc_nav_module.cpp | 547 ++++---------------
src/nav_costmap_module.cpp | 122 +++++
src/nav_module.cpp | 414 ++++++++++++++
src/nav_planner_module.cpp | 14 +
src/opendrive_gp_module.cpp | 123 +++++
15 files changed, 1564 insertions(+), 987 deletions(-)
rename cfg/{ADCNavModule.cfg => NavModule.cfg} (97%)
create mode 100644 include/adc_nav_module/ackermann_lp_module.h
create mode 100644 include/adc_nav_module/opendrive_gp_module.h
create mode 100644 src/ackermann_lp_module.cpp
create mode 100644 src/nav_costmap_module.cpp
create mode 100644 src/nav_module.cpp
create mode 100644 src/nav_planner_module.cpp
create mode 100644 src/opendrive_gp_module.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d326e7d..3c39d7d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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 tf 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)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -86,7 +86,7 @@ find_package(iriutils REQUIRED)
## Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
- cfg/ADCNavModule.cfg
+ cfg/NavModule.cfg
cfg/ADCNavClient.cfg
cfg/ADCNavBtClient.cfg
)
@@ -106,7 +106,7 @@ set(module_bt_name 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 tf 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
# DEPENDS system_lib
)
@@ -124,6 +124,10 @@ include_directories(${iriutils_INCLUDE_DIR})
# Module
add_library(${module_name}
src/adc_nav_module.cpp
+ src/nav_costmap_module.cpp
+ src/nav_planner_module.cpp
+ src/ackermann_lp_module.cpp
+ src/opendrive_gp_module.cpp
)
target_link_libraries(${module_name} ${catkin_LIBRARIES})
@@ -156,12 +160,12 @@ target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name})
#BT_client
-set(bt_client_name adc_nav_bt_client)
-add_executable(${bt_client_name} src/adc_nav_bt_client_alg_node.cpp)
-target_link_libraries(${bt_client_name} ${catkin_LIBRARIES})
-target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
-target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_bt_name}.so)
-add_dependencies(${bt_client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name} ${module_bt_name})
+#set(bt_client_name adc_nav_bt_client)
+#add_executable(${bt_client_name} src/adc_nav_bt_client_alg_node.cpp)
+#target_link_libraries(${bt_client_name} ${catkin_LIBRARIES})
+#target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
+#target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_bt_name}.so)
+#add_dependencies(${bt_client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name} ${module_bt_name})
#############
## Install ##
diff --git a/cfg/ADCNavModule.cfg b/cfg/NavModule.cfg
similarity index 97%
rename from cfg/ADCNavModule.cfg
rename to cfg/NavModule.cfg
index 8341143..1a40824 100755
--- a/cfg/ADCNavModule.cfg
+++ b/cfg/NavModule.cfg
@@ -57,5 +57,5 @@ 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, "ADCNavModuleAlgorithm", "ADCNavModule"))
+exit(gen.generate(PACKAGE, "NavModuleAlgorithm", "NavModule"))
diff --git a/include/adc_nav_module/ackermann_lp_module.h b/include/adc_nav_module/ackermann_lp_module.h
new file mode 100644
index 0000000..6182176
--- /dev/null
+++ b/include/adc_nav_module/ackermann_lp_module.h
@@ -0,0 +1,290 @@
+#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();
+ };
+
+ #endif
diff --git a/include/adc_nav_module/adc_nav_module.h b/include/adc_nav_module/adc_nav_module.h
index 83d2754..e5c35bd 100644
--- a/include/adc_nav_module/adc_nav_module.h
+++ b/include/adc_nav_module/adc_nav_module.h
@@ -2,24 +2,12 @@
#define _ADC_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 <tf/transform_listener.h>
-#include <move_base_msgs/MoveBaseAction.h>
-#include <std_srvs/Empty.h>
+#include <adc_nav_module/nav_module.h>
+#include <adc_nav_module/ackermann_lp_module.h>
+#include <adc_nav_module/opendrive_gp_module.h>
// Dynamic reconfigure header
-#include <adc_nav_module/ADCNavModuleConfig.h>
+#include <adc_nav_module/NavModuleConfig.h>
//States
typedef enum {ADC_NAV_MODULE_IDLE,
@@ -75,49 +63,25 @@ typedef enum {ADC_NAV_MODULE_RUNNING,
#define IGNORE_X_Y_POS 100.0
/**
- * \brief Navigation module
- *
- * This module wraps the basic features of the mobile base navigation of the
- * ADC robot. It provides the following features:
- * * Navigation to a cartesian point (x,y,theta)
- * * Navigation to a point of interest
- * * Navigation through a set of waypoints
- *
- * The current goal can be canceled at any time. If a new goal is given before
- * the previous goals has finished, the previous goal is preempted and the new
- * goal executed.
+ * \brief
*
- * Before accepting any goal, this module tries to change the navigation mode
- * to LOC. If unsuccessfull, it will not accept any goals. However, if the
- * mode is changed afterwards, this module will be unable to detect it.
- *
- * This module also allows to clear the costmaps when desired or at regular
- * intervals to facilitate the navigation, and choose the desired map.
*/
-class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
+class CADCNavModule : public CNavModule,
+ CAckermannLPModule<adc_nav_module::NavModuleConfig>,
+ COpendriveGPModule<adc_nav_module::NavModuleConfig>
{
private:
+ //Variables
/**
* \brief
*
*/
- adc_nav_module::ADCNavModuleConfig config;
- // basic navigation action
- /**
- * \brief
- *
- */
- CModuleAction<move_base_msgs::MoveBaseAction,adc_nav_module::ADCNavModuleConfig> move_base_action;
- /**
- * \brief
- *
- */
- move_base_msgs::MoveBaseGoal move_base_goal;
+ adc_nav_module_state_t state;
/**
* \brief
*
*/
- std::string goal_frame_id;
+ adc_nav_module_status_t status;
/**
* \brief
*
@@ -127,114 +91,17 @@ class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
* \brief
*
*/
- CModuleService<nav_msgs::GetPlan,adc_nav_module::ADCNavModuleConfig> make_plan;
- // dynamic reconfigure
+ double goal_x;
/**
* \brief
*
*/
- CModuleDynReconf<adc_nav_module::ADCNavModuleConfig> move_base_reconf;
-
- // 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
- *
- */
- tf::TransformListener listener;
- /**
- * \brief
- *
- */
- bool transform_position;
- /**
- * \brief
- *
- */
- bool transform_orientation;
- /**
- * \brief
- *
- */
- CROSTimeout tf_timeout;
-
- // costmaps
+ double goal_y;
/**
* \brief
*
*/
- CModuleService<std_srvs::Empty,adc_nav_module::ADCNavModuleConfig> clear_costmaps;
- /**
- * \brief
- *
- */
- ros::Timer clear_costmaps_timer;
- /**
- * \brief
- *
- */
- void clear_costmaps_call(const ros::TimerEvent& event);
- /**
- * \brief
- *
- */
- bool enable_auto_clear;
-
- //Variables
- /**
- * \brief
- *
- */
- adc_nav_module_state_t state;
- /**
- * \brief
- *
- */
- adc_nav_module_status_t status;
+ double goal_yaw;
/**
* \brief
*
@@ -247,16 +114,6 @@ class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
*
*/
void state_machine(void);
- /**
- * \brief
- *
- */
- void reconfigure_callback(adc_nav_module::ADCNavModuleConfig &config, uint32_t level);
- /**
- * \brief
- *
- */
- bool transform_current_pose(void);
public:
/**
@@ -361,23 +218,6 @@ class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
* or not (false).
*/
bool go_to_pose(double x,double y,double yaw,double heading_tol=DEFAULT_HEADING_TOL,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
- /**
- * \brief
- *
- */
- bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,double tolerance=DEFAULT_X_Y_POS_TOL);
- // 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 Stops the current goal
*
@@ -407,125 +247,6 @@ class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
* \return the current status of the module.
*/
adc_nav_module_status_t get_status(void);
- // costmaps functions
- /**
- * \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
- *
- */
- bool are_costmaps_shutdown(void);
- /**
- * \brief
- *
- */
- void shutdown_costmaps(bool shutdown);
- /**
- * \brief
- *
- */
- unsigned int get_max_num_retries(void);
- /**
- * \brief
- *
- */
- void set_max_num_retries(unsigned int retries);
- /**
- * \brief
- *
- */
- double get_planner_frequency(void);
- /**
- * \brief
- *
- */
- void set_planner_frequency(double freq);
- /**
- * \brief
- *
- */
- double get_planner_patience(void);
- /**
- * \brief
- *
- */
- void set_planner_patience(double time);
- /**
- * \brief
- *
- */
- double get_controller_frequency(void);
- /**
- * \brief
- *
- */
- void set_controller_frequency(double freq);
- /**
- * \brief
- *
- */
- double get_controller_patience(void);
- /**
- * \brief
- *
- */
- void set_controller_patience(double time);
/**
* \brief Destructor
*
diff --git a/include/adc_nav_module/nav_costmap_module.h b/include/adc_nav_module/nav_costmap_module.h
index f00387e..ffca204 100644
--- a/include/adc_nav_module/nav_costmap_module.h
+++ b/include/adc_nav_module/nav_costmap_module.h
@@ -1,18 +1,17 @@
-#ifndef _ADC_NAV_COSTMAP_MODULE_H
-#define _ADC_NAV_COSTMAP_MODULE_H
+#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>
-// Dynamic reconfigure header
-#include <adc_nav_module/ADCNavModuleConfig.h>
-
/**
* \brief
*
*/
template <class ModuleCfg>
-class CADCNavCostmapModule
+class CNavCostmapModule
{
private:
@@ -28,7 +27,7 @@ class CADCNavCostmapModule
* \brief Constructor
*
*/
- CADCNavCostmapModule(const std::string &name,const std::string &name_space=std::string(""));
+ CNavCostmapModule(const std::string &name,const std::string &name_space=std::string(""));
/* parameter functions */
/**
* \brief
@@ -54,12 +53,12 @@ class CADCNavCostmapModule
* \brief
*
*/
- void get_footprint(std::vector<std_msgs::Point> &footprint);
+ void get_footprint(std::vector<geometry_msgs::Point> &footprint);
/**
* \brief
*
*/
- void set_footprint(std::vector<std_msgs::Point> &footprint);
+ void set_footprint(std::vector<geometry_msgs::Point> &footprint);
/**
* \brief
*
@@ -109,7 +108,7 @@ class CADCNavCostmapModule
* \brief
*
*/
- void set_origin(double &y,double &y);
+ void set_origin(double &x,double &y);
/**
* \brief
*
@@ -124,7 +123,7 @@ class CADCNavCostmapModule
* \brief Destructor
*
*/
- ~CADCNavModule();
+ ~CNavCostmapModule();
};
#endif
diff --git a/include/adc_nav_module/nav_module.h b/include/adc_nav_module/nav_module.h
index 54ac57b..e6358db 100644
--- a/include/adc_nav_module/nav_module.h
+++ b/include/adc_nav_module/nav_module.h
@@ -14,98 +14,30 @@
// ROS headers
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
-#include <tf/transform_listener.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>
-//States
-typedef enum {NAV_MODULE_IDLE,
- NAV_MODULE_TRANSFORM,
- NAV_MODULE_START,
- NAV_MODULE_WAIT} nav_module_state_t;
+#include <adc_nav_module/nav_costmap_module.h>
-//Status
-/**
- * \brief Possible module status returned by the get_status() function
- *
- * These are the possible status of the module, which are:
- * * NAV_MODULE_RUNNING: indicates that the module is active and operating
- * properly.
- * * NAV_MODULE_SUCCESS: indicates the last navigation goal has been reached
- * successfully.
- * * NAV_MODULE_ACTION_START_FAIL: indicates that the goal could not be started.
- * * NAV_MODULE_TIMEOUT: indicates that the goal did not complete in the
- * allowed time.
- * * NAV_MODULE_FB_WATCHDOG: indicates that feedback has not been received for
- * a long time.
- * * NAV_MODULE_ABORTED: indicates that the navigation goal could not be reached.
- * * NAV_MODULE_PREEMPTED: indicates that the current navigation goal has been
- * canceled by another goal.
- * * NAV_MODULE_REJECTED: indicates that the goal informatio is not valid and the
- * goal will not be executed.
- * * NAV_MODULE_SET_PARAM_FAIL: indicates that it was impossible to set the value
- * of a parameter.
- * * NAV_MODULE_PARAM_NOT_PRESENT: indicates that the parameter to be changed is
- * not present in the remote node.
- * * NAV_MODULE_NO_ODOM: indicates that no odometry information is being received,
- * so it is not possible to get the current position of the
- * robot.
- * * NAV_MODULE_NO_TRANSFORM: indicates that there exist no transform between the
- * desired goal frame and the rest of the TF tree.
- */
-typedef enum {NAV_MODULE_RUNNING,
- NAV_MODULE_SUCCESS,
- NAV_MODULE_ACTION_START_FAIL,
- NAV_MODULE_TIMEOUT,
- NAV_MODULE_FB_WATCHDOG,
- NAV_MODULE_ABORTED,
- NAV_MODULE_PREEMPTED,
- NAV_MODULE_REJECTED,
- NAV_MODULE_SET_PARAM_FAIL,
- NAV_MODULE_PARAM_NOT_PRESENT,
- NAV_MODULE_NO_ODOM,
- NAV_MODULE_NO_TRANSFORM} nav_module_status_t;
-
-#define DEFAULT_HEADING_TOL -1.0
-#define IGNORE_HEADING 3.14159
-#define DEFAULT_X_Y_POS_TOL -1.0
-#define IGNORE_X_Y_POS 100.0
+// Dynamic reconfigure header
+#include <adc_nav_module/NavModuleConfig.h>
/**
* \brief Navigation module
*
- * This module wraps the basic features of the mobile base navigation of the
- * robot. It provides the following features:
- * * Navigation to a cartesian point (x,y,theta)
- * * Navigation to a point of interest
- * * Navigation through a set of waypoints
- *
- * The current goal can be canceled at any time. If a new goal is given before
- * the previous goals has finished, the previous goal is preempted and the new
- * goal executed.
- *
- * Before accepting any goal, this module tries to change the navigation mode
- * to LOC. If unsuccessfull, it will not accept any goals. However, if the
- * mode is changed afterwards, this module will be unable to detect it.
- *
- * This module also allows to clear the costmaps when desired or at regular
- * intervals to facilitate the navigation, and choose the desired map.
*/
-template <class ModuleCfg>
-class CNavModule : public CModule<ModuleCfg>
+class CNavModule : public CModule<adc_nav_module::NavModuleConfig>
{
private:
- /**
- * \brief
- *
- */
- ModuleCfg config;
// basic navigation action
/**
* \brief
*
*/
- CModuleAction<move_base_msgs::MoveBaseAction,ModuleCfg> move_base_action;
+ CModuleAction<move_base_msgs::MoveBaseAction,adc_nav_module::NavModuleConfig> move_base_action;
/**
* \brief
*
@@ -120,18 +52,18 @@ class CNavModule : public CModule<ModuleCfg>
* \brief
*
*/
- bool new_goal;
+ CModuleService<nav_msgs::GetPlan,adc_nav_module::NavModuleConfig> make_plan_service;
+ // dynamic reconfigure
/**
* \brief
*
*/
- CModuleService<nav_msgs::GetPlan,ModuleCfg> make_plan;
- // dynamic reconfigure
+ CModuleDynReconf<adc_nav_module::NavModuleConfig> move_base_reconf;
/**
* \brief
*
*/
- CModuleDynReconf<ModuleCfg> move_base_reconf;
+ adc_nav_module::NavModuleConfig config;
// odometry
/**
@@ -183,29 +115,19 @@ class CNavModule : public CModule<ModuleCfg>
* \brief
*
*/
- tf::TransformListener listener;
- /**
- * \brief
- *
- */
- bool transform_position;
+ tf2_ros::Buffer tf2_buffer;
/**
* \brief
*
*/
- bool transform_orientation;
- /**
- * \brief
- *
- */
- CROSTimeout tf_timeout;
+ tf2_ros::TransformListener tf2_listener;
// costmaps
/**
* \brief
*
*/
- CModuleService<std_srvs::Empty,ModuleCfg> clear_costmaps;
+ CModuleService<std_srvs::Empty,adc_nav_module::NavModuleConfig> clear_costmaps;
/**
* \brief
*
@@ -221,149 +143,69 @@ class CNavModule : public CModule<ModuleCfg>
*
*/
bool enable_auto_clear;
-
- //Variables
/**
* \brief
*
*/
- nav_module_state_t state;
+ CNavCostmapModule<adc_nav_module::NavModuleConfig> local_costmap;
/**
* \brief
*
*/
- nav_module_status_t status;
+ CNavCostmapModule<adc_nav_module::NavModuleConfig> global_costmap;
+ protected:
/**
* \brief
*
*/
- bool cancel_pending;
-
- protected:
+ act_srv_status start_action(double x,double y,double yaw);
/**
* \brief
*
*/
- void state_machine(void);
+ void cancel_action(void);
/**
* \brief
*
*/
- void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
+ action_status get_action_status(void);
/**
* \brief
*
*/
- bool transform_current_pose(void);
-
- public:
+ std::string get_pose_frame_id(void);
/**
- * \brief Constructor
+ * \brief
*
*/
- CNavModule(const std::string &name,const std::string &name_space=std::string(""));
- // basic navigation functions
+ std::string get_goal_frame_id(void);
/**
- * \brief Navigate to a given orientation (theta)
- *
- * This functions rotates the robot about its axis to the given orientation
- * with respect to the reference frame set by the set_goal_frame() function.
- * The current position of the robot is taken from the odometry of the robot.
- *
- * If the local planner supports it, it is possible to set the desired
- * heading goal tolerance. By default, the default tolerance of the planner
- * is used. If the planner does not support changing the goal tolerance, this
- * function will fail if any tolerance other than DEFAULT_HEADING_TOL is
- * used.
- *
- * This function may fail for the following reasons:
- * * The heading tolerance parameter does not exist in the local planner.
- * * The heading tolerance exists but it could not be set to the desired
- * value.
- *
- * \param yaw desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * \param heading_tol heading tolerance for the goal. By default it is set to
- * -1, which means to use the default planner tolerance.
+ * \brief
*
- * \return a boolean indicating wheather the request has been successfull (true)
- * or not (false).
*/
- bool go_to_orientation(double yaw,double heading_tol=DEFAULT_HEADING_TOL);
+ virtual void state_machine(void)=0;
/**
- * \brief Navigate to a given position (x,y)
- *
- * This functions moves the robot to the given position preserving the final
- * orientation with respect to the reference frame set by the set_goal_frame()
- * function. The current orientation of the robot is taken from the odometry
- * of the robot.
- *
- * If the local planner supports it, it is possible to set the desired
- * position goal tolerance. By default, the default tolerance of the planner
- * is used. If the planner does not support changing the goal tolerance, this
- * function will fail if any tolerance other than DEFAULT_X_Y_POS_TOL is
- * used.
- *
- * This function may fail for the following reasons:
- * * The position tolerance parameter does not exist in the local planner.
- * * The position tolerance exists but it could not be set to the desired
- * value.
- *
- * \param x desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * \param y desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * \param x_y_pos_tol position tolerance for the goal. By default it is set to
- * -1, which means to use the default planner tolerance.
+ * \brief
*
- * \return a boolean indicating wheather the request has been successfull (true)
- * or not (false).
*/
- bool go_to_position(double x,double y,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
+ void reconfigure_callback(adc_nav_module::NavModuleConfig &config, uint32_t level);
/**
- * \brief Navigate to a given pose (x,y,theta)
- *
- * This functions moves the robot to the given pose preserving with respect
- * to the reference frame set by the set_goal_frame() function.
- *
- * If the local planner supports it, it is possible to set the desired
- * pose goal tolerance. By default, the default tolerance of the planner
- * is used. If the planner does not support changing the goal tolerance, this
- * function will fail if any tolerance other than DEFAULT_X_Y_POS_TOL and
- * DEFAULT_HEADING_TOL are used.
- *
- * This function may fail for the following reasons:
- * * The position and/or heading tolerance parameter do not exist in the
- * local planner.
- * * The position and/or heading tolerance exists but it could not be set
- * to the desired value.
- *
- * \param x desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * \param y desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * \param yaw desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
+ * \brief
*
- * \param heading_tol heading tolerance for the goal. By default it is set to
- * -1, which means to use the default planner tolerance.
- * \param x_y_pos_tol position tolerance for the goal. By default it is set to
- * -1, which means to use the default planner tolerance.
+ */
+ bool transform_current_pose(void);
+
+ public:
+ /**
+ * \brief Constructor
*
- * \return a boolean indicating wheather the request has been successfull (true)
- * or not (false).
*/
- bool go_to_pose(double x,double y,double yaw,double heading_tol=DEFAULT_HEADING_TOL,double x_y_pos_tol=DEFAULT_X_Y_POS_TOL);
+ 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,double tolerance=DEFAULT_X_Y_POS_TOL);
+ 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
@@ -376,36 +218,6 @@ class CNavModule : public CModule<ModuleCfg>
* of the robot.
*/
void set_goal_frame(std::string &frame_id);
- /**
- * \brief Stops the current goal
- *
- * This functions stops the motion of the robot, whatever the type of goal
- * that has been given. This functions signals the stop to the navigation
- * module, but the is_finished() function must be used to know when the
- * navigation is actually stopped.
- */
- void stop(void);
- /**
- * \brief Checks if the last goal has finished or not.
- *
- * This function must be used to know when the last navigation goal has
- * ended, either successfully or by any error.
- *
- * \return a boolean indicating whether the last navigation goal has ended
- * (true) or not (false), reagardless of its success or not.
- */
- bool is_finished(void);
- /**
- * \brief Returns the current state
- *
- * This function returns the current status of the navigation module. It returns
- * one of the nav_module_status_t values. This can be called at any time, but is
- * has only meaning when a goal has just finished.
- *
- * \return the current status of the module.
- */
- nav_module_status_t get_status(void);
- // costmaps functions
/**
* \brief Clears the costmaps
*
@@ -468,62 +280,72 @@ class CNavModule : public CModule<ModuleCfg>
* \brief
*
*/
- bool are_costmaps_shutdown(void);
+ 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
*
*/
- void shutdown_costmaps(bool shutdown);
+ dyn_reconf_status_t set_max_num_retries(unsigned int &retries);
/**
* \brief
*
*/
- unsigned int get_max_num_retries(void);
+ dyn_reconf_status_t get_planner_frequency(double &freq);
/**
* \brief
*
*/
- void set_max_num_retries(unsigned int retries);
+ dyn_reconf_status_t set_planner_frequency(double &freq);
/**
* \brief
*
*/
- double get_planner_frequency(void);
+ dyn_reconf_status_t get_planner_patience(double &time);
/**
* \brief
*
*/
- void set_planner_frequency(double freq);
+ dyn_reconf_status_t set_planner_patience(double &time);
/**
* \brief
*
*/
- double get_planner_patience(void);
+ dyn_reconf_status_t get_controller_frequency(double &freq);
/**
* \brief
*
*/
- void set_planner_patience(double time);
+ dyn_reconf_status_t set_controller_frequency(double &freq);
/**
* \brief
*
*/
- double get_controller_frequency(void);
+ dyn_reconf_status_t get_controller_patience(double &time);
/**
* \brief
*
*/
- void set_controller_frequency(double freq);
+ dyn_reconf_status_t set_controller_patience(double &time);
/**
* \brief
*
*/
- double get_controller_patience(void);
+ const CNavCostmapModule<adc_nav_module::NavModuleConfig> &get_local_costmap(void) const;
/**
* \brief
*
*/
- void set_controller_patience(double time);
+ const CNavCostmapModule<adc_nav_module::NavModuleConfig> &get_global_costmap(void) const;
/**
* \brief Destructor
*
diff --git a/include/adc_nav_module/nav_planner_module.h b/include/adc_nav_module/nav_planner_module.h
index b85f35c..5748309 100644
--- a/include/adc_nav_module/nav_planner_module.h
+++ b/include/adc_nav_module/nav_planner_module.h
@@ -1,18 +1,15 @@
-#ifndef _ADC_NAV_PLANNER_MODULE_H
-#define _ADC_NAV_PLANNER_MODULE_H
+#ifndef _NAV_PLANNER_MODULE_H
+#define _NAV_PLANNER_MODULE_H
// IRI ROS headers
#include <iri_ros_tools/module_dyn_reconf.h>
-// Dynamic reconfigure header
-#include <adc_nav_module/ADCNavModuleConfig.h>
-
/**
* \brief
*
*/
template <class ModuleCfg>
-class CADCNavPlannerModule
+class CNavPlannerModule
{
private:
@@ -28,13 +25,13 @@ class CADCNavPlannerModule
* \brief Constructor
*
*/
- CADCNavPlannerModule(const std::string &name,const std::string &name_space=std::string(""));
+ CNavPlannerModule(const std::string &name,const std::string &name_space=std::string(""));
/* parameter functions */
/**
* \brief Destructor
*
*/
- ~CADCNavModule();
+ ~CNavPlannerModule();
};
#endif
diff --git a/include/adc_nav_module/opendrive_gp_module.h b/include/adc_nav_module/opendrive_gp_module.h
new file mode 100644
index 0000000..84033b0
--- /dev/null
+++ b/include/adc_nav_module/opendrive_gp_module.h
@@ -0,0 +1,50 @@
+#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 */
+ bool multi_hypothesis(void);
+ void multy_hypothesis(bool enable);
+ cost_type_t get_cost_type(void);
+ void set_cost_type(cost_type_t type);
+ std::string get_opendrive_map(void);
+ void set_opendrive_map(const std::string &map_file);
+ std::string get_opendrive_frame(void);
+ void get_popendrive_frame(const std::string &frame);
+ double get_distance_tolerance(void);
+ void set_distance_tolerance(double tolerance);
+ double get_angle_tolerance(void);
+ void set_angle_tolerance(double tolerance);
+ double get_resolution(void);
+ void set_resolution(double resolution);
+ double get_scale_factor(void);
+ void set_scale_factor(double scale);
+ double get_min_road_length(void);
+ void set_min_raod_length(double length);
+ /**
+ * \brief Destructor
+ *
+ */
+ ~COpendriveGPModule();
+ };
+
+ #endif
diff --git a/package.xml b/package.xml
index 8d4eea1..eabd987 100644
--- a/package.xml
+++ b/package.xml
@@ -47,7 +47,7 @@
<build_depend>std_srvs</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
- <build_depend>tf</build_depend>
+ <build_depend>tf2_ros</build_depend>
<build_depend>iri_base_algorithm</build_depend>
<build_depend>iri_base_bt_client</build_depend>
<build_depend>iri_behaviortree</build_depend>
@@ -60,7 +60,7 @@
<run_depend>std_srvs</run_depend>
<run_depend>move_base_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
- <run_depend>tf</run_depend>
+ <run_depend>tf2_ros</run_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>iri_base_bt_client</run_depend>
<run_depend>iri_behaviortree</run_depend>
diff --git a/src/ackermann_lp_module.cpp b/src/ackermann_lp_module.cpp
new file mode 100644
index 0000000..a66448f
--- /dev/null
+++ b/src/ackermann_lp_module.cpp
@@ -0,0 +1,326 @@
+#include <adc_nav_module/ackermann_lp_module.h>
+
+template <class ModuleCfg>
+CAckermannLPModule<ModuleCfg>::CAckermannLPModule(const std::string &name,const std::string &name_space)
+{
+
+}
+
+template <class ModuleCfg>
+/* parameter functions */
+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()
+{
+
+}
diff --git a/src/adc_nav_module.cpp b/src/adc_nav_module.cpp
index da23f87..f97c377 100644
--- a/src/adc_nav_module.cpp
+++ b/src/adc_nav_module.cpp
@@ -1,55 +1,20 @@
#include <adc_nav_module/adc_nav_module.h>
#include <tf/transform_datatypes.h>
-CADCNavModule::CADCNavModule(const std::string &name,const std::string &name_space) : CModule(name,name_space),
- move_base_action("move_base",this->module_nh.getNamespace()),
- move_base_reconf("move_base_reconf",this->module_nh.getNamespace()),
- palLocalPlanner_reconf("palLocalPlanner_reconf",this->module_nh.getNamespace()),
- global_inflation_layer_reconf("global_inflation_layer_reconf",this->module_nh.getNamespace()),
- local_inflation_layer_reconf("local_inflation_layer_reconf",this->module_nh.getNamespace()),
- global_obstacle_laser_layer_reconf("global_obstacle_laser_layer_reconf",this->module_nh.getNamespace()),
- global_obstacle_rgbd_layer_reconf("global_obstacle_rgbd_layer_reconf",this->module_nh.getNamespace()),
- clear_costmaps("clear_costmaps",this->module_nh.getNamespace())
+CADCNavModule::CADCNavModule(const std::string &name,const std::string &name_space) : CNavModule(name,name_space),
+ CAckermannLPModule(name,name_space),
+ COpendriveGPModule(name,name_space)
{
this->start_operation();
//Variables
- this->state=ADC_NAV_MODULE_SET_LOC_MODE;
+ this->state=ADC_NAV_MODULE_IDLE;
this->status=ADC_NAV_MODULE_SUCCESS;
this->new_goal=false;
+ this->goal_x=0.0;
+ this->goal_y=0.0;
+ this->goal_yaw=0.0;
this->cancel_pending=false;
- this->path_available = false;
-
- // initialize odometry subscriber
- if(!this->module_nh.getParam("odom_watchdog_time_s", this->config.odom_watchdog_time_s))
- {
- ROS_WARN("CADCNavModule::CADCNavModule: 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,&CADCNavModule::odom_callback,this);
-
- // initialize path subscriber
- this->path_subscriber = this->module_nh.subscribe("path", 1, &CADCNavModule::path_callback, this);
-
- // tf listener
- this->transform_position=false;
- this->transform_orientation=false;
-
- // costmaps
- if(!this->module_nh.getParam("clear_costmap_auto_clear_rate_hz", this->config.clear_costmap_auto_clear_rate_hz))
- {
- ROS_WARN("CADCNavModule::CADCNavModule: 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),&CADCNavModule::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),&CADCNavModule::clear_costmaps_call,this);
- // this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CADCNavModule::clear_costmaps_call,this);
- this->clear_costmaps_timer.stop();
- this->enable_auto_clear=false;
}
/* State machine */
@@ -58,255 +23,120 @@ void CADCNavModule::state_machine(void)
switch(this->state)
{
case ADC_NAV_MODULE_IDLE: ROS_DEBUG("CADCNavModule: Idle");
- if(this->new_goal)
- {
- this->new_goal=false;
- if(this->transform_position || this->transform_orientation)
- {
- this->state=ADC_NAV_MODULE_TRANSFORM;
- this->tf_timeout.start(ros::Duration(this->config.tf_timeout_time_s));
- }
- else
- this->state=ADC_NAV_MODULE_START;
- this->status=ADC_NAV_MODULE_RUNNING;
- }
- else
- this->state=ADC_NAV_MODULE_IDLE;
- break;
+ if(this->new_goal)
+ {
+ this->new_goal=false;
+ this->state=ADC_NAV_MODULE_TRANSFORM;
+ this->status=ADC_NAV_MODULE_RUNNING;
+ }
+ else
+ this->state=ADC_NAV_MODULE_IDLE;
+ break;
case ADC_NAV_MODULE_TRANSFORM: ROS_DEBUG("CADCNavModule: Transform");
- if(this->current_pose.header.frame_id!=this->goal_frame_id)
- {
- if(this->tf_timeout.timed_out())
- {
- this->status=ADC_NAV_MODULE_NO_TRANSFORM;
- this->state=ADC_NAV_MODULE_IDLE;
- }
- else if(this->transform_current_pose())
- this->state=ADC_NAV_MODULE_START;
- else
- this->state=ADC_NAV_MODULE_TRANSFORM;
- }
- else
- {
- this->transform_position=false;
- this->transform_orientation=false;
- this->state=ADC_NAV_MODULE_START;
- }
- break;
+ if(this->get_pose_frame_id()!=this->get_goal_frame_id())
+ {
+ if(this->transform_current_pose())
+ this->state=ADC_NAV_MODULE_START;
+ else
+ this->state=ADC_NAV_MODULE_IDLE;
+ }
+ else
+ this->state=ADC_NAV_MODULE_START;
+ break;
case ADC_NAV_MODULE_START: ROS_DEBUG("CADCNavModule: Start");
- switch(this->move_base_action.make_request(this->move_base_goal))
- {
- case ACT_SRV_SUCCESS: this->state=ADC_NAV_MODULE_WAIT;
- ROS_DEBUG("CADCNavModule : goal start successfull");
- break;
- case ACT_SRV_PENDING: this->state=ADC_NAV_MODULE_START;
- ROS_WARN("CADCNavModule : goal start pending");
- break;
- case ACT_SRV_FAIL: this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_ACTION_START_FAIL;
- ROS_ERROR("CADCNavModule: goal start failed");
- break;
- }
+ 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");
+ break;
+ case ACT_SRV_PENDING: this->state=ADC_NAV_MODULE_START;
+ ROS_WARN("CADCNavModule : goal start pending");
+ break;
+ case ACT_SRV_FAIL: this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_ACTION_START_FAIL;
+ ROS_ERROR("CADCNavModule: goal start failed");
break;
+ }
+ break;
case ADC_NAV_MODULE_WAIT: ROS_DEBUG("CADCNavModule : Wait");
- switch(this->move_base_action.get_state())
- {
- case ACTION_IDLE:
- case ACTION_RUNNING: ROS_DEBUG("CADCNavModule : action running");
- this->state=ADC_NAV_MODULE_WAIT;
- break;
- case ACTION_SUCCESS: ROS_INFO("CADCNavModule : action ended successfully");
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_SUCCESS;
- this->move_base_action.stop_timeout();
- break;
- case ACTION_TIMEOUT: ROS_ERROR("CADCNavModule : action did not finish in the allowed time");
- this->move_base_action.cancel();
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_TIMEOUT;
- this->move_base_action.stop_timeout();
- break;
- case ACTION_FB_WATCHDOG: ROS_ERROR("CADCNavModule : No feedback received for a long time");
- this->move_base_action.cancel();
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_FB_WATCHDOG;
- this->move_base_action.stop_timeout();
- break;
- case ACTION_ABORTED: ROS_ERROR("CADCNavModule : Action failed to complete");
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_ABORTED;
- this->move_base_action.stop_timeout();
- break;
- case ACTION_PREEMPTED: ROS_ERROR("CADCNavModule : Action was interrupted by another request");
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_PREEMPTED;
- this->move_base_action.stop_timeout();
- break;
- case ACTION_REJECTED: ROS_ERROR("CADCNavModule : Action was rejected by server");
- this->state=ADC_NAV_MODULE_IDLE;
- this->status=ADC_NAV_MODULE_REJECTED;
- this->move_base_action.stop_timeout();
- break;
- }
- if(this->new_goal)
- {
- this->new_goal=false;
- if(this->transform_position || this->transform_orientation)
- {
- this->state=ADC_NAV_MODULE_TRANSFORM;
- this->tf_timeout.start(ros::Duration(this->config.tf_timeout_time_s));
- }
- else
- this->state=ADC_NAV_MODULE_START;
- this->status=ADC_NAV_MODULE_RUNNING;
- }
- if(this->cancel_pending)
- {
- this->cancel_pending=false;
- this->move_base_action.cancel();
- }
- break;
- }
-}
-
-void CADCNavModule::reconfigure_callback(adc_nav_module::ADCNavModuleConfig &config, uint32_t level)
-{
- ROS_DEBUG("CADCNavModule : 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 CADCNavModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
-{
- ROS_DEBUG("CADCNavModule: 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 CADCNavModule::path_callback(const nav_msgs::Path::ConstPtr& msg){
- ROS_DEBUG("CADCNavModule: path callback");
- this->lock();
- this->path_msg = *msg;
- this->path_available = true;
- this->unlock();
-}
-
-bool CADCNavModule::transform_current_pose(void)
-{
- geometry_msgs::PoseStamped pose;
-
- try{
- if(this->listener.canTransform(this->goal_frame_id,this->current_pose.header.frame_id,this->current_pose.header.stamp))
- {
- this->listener.transformPose(this->goal_frame_id,this->current_pose,pose);
- if(this->transform_position)
+ switch(this->get_action_status())
{
- 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->transform_position=false;
+ case ACTION_IDLE:
+ case ACTION_RUNNING: ROS_DEBUG("CADCNavModule : action running");
+ this->state=ADC_NAV_MODULE_WAIT;
+ break;
+ case ACTION_SUCCESS: ROS_INFO("CADCNavModule : action ended successfully");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_SUCCESS;
+ break;
+ case ACTION_TIMEOUT: ROS_ERROR("CADCNavModule : action did not finish in the allowed time");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_TIMEOUT;
+ break;
+ case ACTION_FB_WATCHDOG: ROS_ERROR("CADCNavModule : No feedback received for a long time");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_FB_WATCHDOG;
+ break;
+ case ACTION_ABORTED: ROS_ERROR("CADCNavModule : Action failed to complete");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_ABORTED;
+ break;
+ case ACTION_PREEMPTED: ROS_ERROR("CADCNavModule : Action was interrupted by another request");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_PREEMPTED;
+ break;
+ case ACTION_REJECTED: ROS_ERROR("CADCNavModule : Action was rejected by server");
+ this->state=ADC_NAV_MODULE_IDLE;
+ this->status=ADC_NAV_MODULE_REJECTED;
+ break;
}
- if(this->transform_orientation)
+ if(this->new_goal)
{
- this->move_base_goal.target_pose.pose.orientation=pose.pose.orientation;
- this->transform_orientation=false;
+ this->new_goal=false;
+ this->state=ADC_NAV_MODULE_TRANSFORM;
}
- return true;
- }
- else
- return false;
- }catch(...){
- return false;
- }
-
- return false;
-}
-
-void CADCNavModule::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("CADCNavModule: Costmaps cleared sucessfully");
- break;
- case ACT_SRV_PENDING: ROS_WARN("CADCNavModule: Costmaps not yet cleared");
- break;
- case ACT_SRV_FAIL: ROS_ERROR("CADCNavModule: Impossible to clear costmaps");
- break;
+ if(this->cancel_pending)
+ {
+ this->cancel_pending=false;
+ this->cancel_action();
+ }
+ break;
}
- this->unlock();
}
// basic navigation functions
bool CADCNavModule::go_to_orientation(double yaw,double heading_tol)
{
+ double xy_tol,yaw_tol;
+
this->lock();
if(this->config.move_base_cancel_prev && !this->is_finished())
- this->move_base_action.cancel();
+ this->cancel_action();
if(heading_tol!=DEFAULT_HEADING_TOL)
{
- if(!this->move_base_reconf.set_parameter("yaw_goal_tolerance",heading_tol))
+ if(this->get_goal_tolerances(xy_tol,yaw_tol))
{
- if(this->move_base_reconf.get_status()==DYN_RECONF_NO_CHANGE)
- {
- this->status=ADC_NAV_MODULE_SET_PARAM_FAIL;
- this->unlock();
- return false;
- }
- else// the parameter does not exist, ignore it and read the current position from odom
+ yaw_tol=heading_tol;
+ if(this->set_goal_tolerances(xy_tol,yaw_tol))
{
- this->status=ADC_NAV_MODULE_PARAM_NOT_PRESENT;
this->unlock();
return false;
}
}
+ else
+ {
+ this->unlock();
+ return false;
+ }
}
- // get the current position
- if(this->odom_watchdog.is_active())
- {
- this->status=ADC_NAV_MODULE_NO_ODOM;
- this->transform_position=false;
- this->unlock();
- return false;
- }
- else
- this->transform_position=true;
/* store the new information goal */
- 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.orientation=tf::createQuaternionMsgFromYaw(yaw);
+ this->goal_x=0.0;
+ this->goal_y=0.0;
+ this->goal_yaw=yaw;
this->new_goal=true;
- this->path_available = false;
this->unlock();
return true;
@@ -314,45 +144,33 @@ bool CADCNavModule::go_to_orientation(double yaw,double heading_tol)
bool CADCNavModule::go_to_position(double x,double y,double x_y_pos_tol)
{
+ double xy_tol,yaw_tol;
+
this->lock();
if(this->config.move_base_cancel_prev && !this->is_finished())
- this->move_base_action.cancel();
+ this->cancel_action();
if(x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
{
- if(!this->move_base_reconf.set_parameter("xy_goal_tolerance",x_y_pos_tol))
+ if(this->get_goal_tolerances(xy_tol,yaw_tol))
{
- if(this->move_base_reconf.get_status()==DYN_RECONF_NO_CHANGE)
+ xy_tol=x_y_pos_tol;
+ if(this->set_goal_tolerances(xy_tol,yaw_tol))
{
- this->status=ADC_NAV_MODULE_SET_PARAM_FAIL;
- this->unlock();
- return false;
- }
- else// the parameter does not exist, ignore it and read the current position from odom
- {
- this->status=ADC_NAV_MODULE_PARAM_NOT_PRESENT;
this->unlock();
return false;
}
}
+ else
+ {
+ this->unlock();
+ return false;
+ }
}
- // get the current orientation
- if(this->odom_watchdog.is_active())
- {
- this->status=ADC_NAV_MODULE_NO_ODOM;
- this->transform_orientation=false;
- this->unlock();
- return false;
- }
- else
- this->transform_orientation=true;
/* store the new information goal */
- 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;
+ this->goal_x=x;
+ this->goal_y=y;
+ this->goal_yaw=0.0;
this->new_goal=true;
- this->path_available = false;
this->unlock();
return true;
@@ -360,67 +178,41 @@ bool CADCNavModule::go_to_position(double x,double y,double x_y_pos_tol)
bool CADCNavModule::go_to_pose(double x,double y,double yaw,double heading_tol,double x_y_pos_tol)
{
+ double xy_tol,yaw_tol;
+
this->lock();
if(this->config.move_base_cancel_prev && !this->is_finished())
- this->move_base_action.cancel();
- if(heading_tol!=DEFAULT_HEADING_TOL)
+ this->cancel_action();
+ if(heading_tol!=DEFAULT_HEADING_TOL || x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
{
- if(!this->move_base_reconf.set_parameter("yaw_goal_tolerance",heading_tol))
+ if(this->get_goal_tolerances(xy_tol,yaw_tol))
{
- if(this->move_base_reconf.get_status()==DYN_RECONF_NO_CHANGE)
- {
- this->status=ADC_NAV_MODULE_SET_PARAM_FAIL;
- this->unlock();
- return false;
- }
- else// the parameter does not exist, ignore it and read the current position from odom
+ if(x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
+ xy_tol=x_y_pos_tol;
+ if(heading_tol!=DEFAULT_HEADING_TOL)
+ yaw_tol=heading_tol;
+ if(this->set_goal_tolerances(xy_tol,yaw_tol))
{
- this->status=ADC_NAV_MODULE_PARAM_NOT_PRESENT;
this->unlock();
return false;
}
}
- }
- if(x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
- {
- if(!this->move_base_reconf.set_parameter("xy_goal_tolerance",x_y_pos_tol))
+ else
{
- if(this->move_base_reconf.get_status()==DYN_RECONF_NO_CHANGE)
- {
- this->status=ADC_NAV_MODULE_SET_PARAM_FAIL;
- this->unlock();
- return false;
- }
- else// the parameter does not exist, ignore it and read the current position from odom
- {
- this->status=ADC_NAV_MODULE_PARAM_NOT_PRESENT;
- this->unlock();
- return false;
- }
+ this->unlock();
+ return false;
}
}
/* store the new information goal */
- 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;
- this->move_base_goal.target_pose.pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
+ this->goal_x=x;
+ this->goal_y=y;
+ this->goal_yaw=yaw;
this->new_goal=true;
- this->path_available = false;
this->unlock();
return true;
}
-// common functions
-void CADCNavModule::set_goal_frame(std::string &frame_id)
-{
- this->lock();
- this->goal_frame_id=frame_id;
- this->unlock();
-}
-
void CADCNavModule::stop(void)
{
if(this->state!=ADC_NAV_MODULE_IDLE)
@@ -440,103 +232,6 @@ adc_nav_module_status_t CADCNavModule::get_status(void)
return this->status;
}
-// costmap functions
-bool CADCNavModule::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("CADCNavModule: Costmaps cleared sucessfully");
- this->unlock();
- return true;
- case ACT_SRV_PENDING: ROS_WARN("CADCNavModule: Costmaps not yet cleared");
- this->unlock();
- return false;
- break;
- case ACT_SRV_FAIL: ROS_ERROR("CADCNavModule: Impossible to clear costmaps");
- this->unlock();
- return false;
- break;
- }
- }
- else
- {
- this->unlock();
- return false;
- }
-
- this->unlock();
- return false;
-}
-
-void CADCNavModule::costmaps_enable_auto_clear(double rate_hz)
-{
- this->lock();
- if(!this->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;
- }
- this->unlock();
-}
-
-void CADCNavModule::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 CADCNavModule::costamp_is_auto_clear_enabled(void)
-{
- return this->enable_auto_clear;
-}
-
-geometry_msgs::Pose CADCNavModule::getCurrentPose(void) {
- return this->current_pose.pose;
-}
-
-double CADCNavModule::getGoalDistance(void)
-{
- this->lock();
- if (!this->path_available)
- {
- ROS_WARN("CADCNavModule::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("CADCNavModule::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;
-}
-
CADCNavModule::~CADCNavModule()
{
- if(!this->move_base_action.is_finished())
- {
- this->move_base_action.cancel();
- while(!this->move_base_action.is_finished());
- }
}
diff --git a/src/nav_costmap_module.cpp b/src/nav_costmap_module.cpp
new file mode 100644
index 0000000..5554a32
--- /dev/null
+++ b/src/nav_costmap_module.cpp
@@ -0,0 +1,122 @@
+#include <adc_nav_module/nav_costmap_module.h>
+
+template <class ModuleCfg>
+CNavCostmapModule<ModuleCfg>::CNavCostmapModule(const std::string &name,const std::string &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()
+{
+
+}
+
diff --git a/src/nav_module.cpp b/src/nav_module.cpp
new file mode 100644
index 0000000..aa769b2
--- /dev/null
+++ b/src/nav_module.cpp
@@ -0,0 +1,414 @@
+#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()),
+ clear_costmaps("clear_costmaps",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->gloal_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;
+ 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 &CNavModule::get_local_costmap(void) const
+{
+ return &this->local_costmap;
+}
+
+const CNavCostmapModule &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
new file mode 100644
index 0000000..16cee64
--- /dev/null
+++ b/src/nav_planner_module.cpp
@@ -0,0 +1,14 @@
+#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()
+{
+
+}
+
diff --git a/src/opendrive_gp_module.cpp b/src/opendrive_gp_module.cpp
new file mode 100644
index 0000000..c7f907a
--- /dev/null
+++ b/src/opendrive_gp_module.cpp
@@ -0,0 +1,123 @@
+#include <adc_nav_module/opendrive_gp_module.h>
+
+template<class ModuleCfg>
+COpendriveGPModule<ModuleCfg>::COpendriveGPModule(const std::string &name,const std::string &name_space)
+{
+
+}
+
+/* parameter functions */
+template<class ModuleCfg>
+bool COpendriveGPModule<ModuleCfg>::multi_hypothesis(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::multy_hypothesis(bool enable)
+{
+
+}
+
+template<class ModuleCfg>
+cost_type_t COpendriveGPModule<ModuleCfg>::get_cost_type(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_cost_type(cost_type_t type)
+{
+
+}
+
+template<class ModuleCfg>
+std::string COpendriveGPModule<ModuleCfg>::get_opendrive_map(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_opendrive_map(const std::string &map_file)
+{
+
+}
+
+template<class ModuleCfg>
+std::string COpendriveGPModule<ModuleCfg>::get_opendrive_frame(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::get_popendrive_frame(const std::string &frame)
+{
+
+}
+
+template<class ModuleCfg>
+double COpendriveGPModule<ModuleCfg>::get_distance_tolerance(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_distance_tolerance(double tolerance)
+{
+
+}
+
+template<class ModuleCfg>
+double COpendriveGPModule<ModuleCfg>::get_angle_tolerance(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_angle_tolerance(double tolerance)
+{
+
+}
+
+template<class ModuleCfg>
+double COpendriveGPModule<ModuleCfg>::get_resolution(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_resolution(double resolution)
+{
+
+}
+
+template<class ModuleCfg>
+double COpendriveGPModule<ModuleCfg>::get_scale_factor(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_scale_factor(double scale)
+{
+
+}
+
+template<class ModuleCfg>
+double COpendriveGPModule<ModuleCfg>::get_min_road_length(void)
+{
+
+}
+
+template<class ModuleCfg>
+void COpendriveGPModule<ModuleCfg>::set_min_raod_length(double length)
+{
+
+}
+
+template<class ModuleCfg>
+COpendriveGPModule<ModuleCfg>::~COpendriveGPModule()
+{
+
+}
+
--
GitLab