From 73fc69cb843311ef737de7ca70897ca4a2098d28 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Mon, 1 Nov 2021 09:03:50 +0100
Subject: [PATCH] Implemented the functions to handle the footprint in the
costmap module. Split the original package into two: this one has the
implementation of the generic navigation module.
---
CMakeLists.txt | 52 +-
cfg/ADCNavBtClient.cfg | 56 --
cfg/ADCNavClient.cfg | 68 --
cfg/ADCNavModule.cfg | 44 --
config/adc_nav_module_default.yaml | 24 -
include/adc_nav_bt_client_alg_node.h | 159 -----
include/adc_nav_client_alg.h | 131 ----
include/adc_nav_client_alg_node.h | 119 ----
.../iri_adc_nav_module/adc_nav_bt_module.h | 650 -----------------
include/iri_adc_nav_module/adc_nav_module.h | 262 -------
.../ackermann_lp_module.h | 2 +-
.../nav_costmap_module.h | 22 +
.../nav_module.h | 2 +-
.../nav_planner_module.h | 0
.../opendrive_gp_module.h | 2 +-
launch/adc_nav_bt_client.launch | 57 --
launch/adc_nav_client.launch | 59 --
package.xml | 10 +-
setup.py | 2 +-
src/adc_nav_bt_client_alg_node.cpp | 105 ---
src/adc_nav_bt_module.cpp | 655 ------------------
src/adc_nav_client_alg.cpp | 23 -
src/adc_nav_client_alg_node.cpp | 139 ----
src/adc_nav_module.cpp | 239 -------
src/iri_adc_nav_module/dyn_params.pyc | Bin 1678 -> 0 bytes
.../__init__.py | 0
.../dyn_params.py | 8 +
src/xml/bt_test.xml | 184 -----
28 files changed, 52 insertions(+), 3022 deletions(-)
delete mode 100755 cfg/ADCNavBtClient.cfg
delete mode 100755 cfg/ADCNavClient.cfg
delete mode 100755 cfg/ADCNavModule.cfg
delete mode 100644 config/adc_nav_module_default.yaml
delete mode 100644 include/adc_nav_bt_client_alg_node.h
delete mode 100644 include/adc_nav_client_alg.h
delete mode 100644 include/adc_nav_client_alg_node.h
delete mode 100644 include/iri_adc_nav_module/adc_nav_bt_module.h
delete mode 100644 include/iri_adc_nav_module/adc_nav_module.h
rename include/{iri_adc_nav_module => iri_nav_module}/ackermann_lp_module.h (99%)
rename include/{iri_adc_nav_module => iri_nav_module}/nav_costmap_module.h (90%)
rename include/{iri_adc_nav_module => iri_nav_module}/nav_module.h (99%)
rename include/{iri_adc_nav_module => iri_nav_module}/nav_planner_module.h (100%)
rename include/{iri_adc_nav_module => iri_nav_module}/opendrive_gp_module.h (99%)
delete mode 100644 launch/adc_nav_bt_client.launch
delete mode 100644 launch/adc_nav_client.launch
delete mode 100644 src/adc_nav_bt_client_alg_node.cpp
delete mode 100644 src/adc_nav_bt_module.cpp
delete mode 100644 src/adc_nav_client_alg.cpp
delete mode 100644 src/adc_nav_client_alg_node.cpp
delete mode 100644 src/adc_nav_module.cpp
delete mode 100644 src/iri_adc_nav_module/dyn_params.pyc
rename src/{iri_adc_nav_module => iri_nav_module}/__init__.py (100%)
rename src/{iri_adc_nav_module => iri_nav_module}/dyn_params.py (87%)
delete mode 100644 src/xml/bt_test.xml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9695ebd..a3d2b20 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
-project(iri_adc_nav_module)
+project(iri_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 iri_adc_msgs)
+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_adc_msgs costmap_2d)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -85,11 +85,8 @@ catkin_python_setup()
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
- cfg/ADCNavModule.cfg
- cfg/ADCNavClient.cfg
- cfg/ADCNavBtClient.cfg
-)
+# generate_dynamic_reconfigure_options(
+# )
###################################
## catkin specific configuration ##
@@ -100,13 +97,12 @@ 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 iri_adc_nav_module)
-set(module_bt_name iri_adc_nav_module_bt)
+set(module_name iri_nav_module)
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 iri_adc_msgs
+ LIBRARIES ${module_name}
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf2_ros nav_msgs iri_base_algorithm iri_adc_msgs costmap_2d
# DEPENDS system_lib
)
@@ -123,8 +119,13 @@ include_directories(${iriutils_INCLUDE_DIR})
## Declare a C++ libraryi
# Module
add_library(${module_name}
- src/adc_nav_module.cpp
+ include/${PROJECT_NAME}/ackermann_lp_module.h
+ include/${PROJECT_NAME}/nav_costmap_module.h
+ include/${PROJECT_NAME}/nav_module.h
+ include/${PROJECT_NAME}/nav_planner_module.h
+ include/${PROJECT_NAME}/opendrive_gp_module.h
)
+set_target_properties(${module_name} PROPERTIES LINKER_LANGUAGE CXX)
target_link_libraries(${module_name} ${catkin_LIBRARIES})
target_link_libraries(${module_name} ${iriutils_LIBRARY})
@@ -134,34 +135,9 @@ target_link_libraries(${module_name} ${iriutils_LIBRARY})
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
-add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+# add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS})
## Declare a C++ executable
-#Module_client
-set(client_name adc_nav_client)
-add_executable(${client_name} src/adc_nav_client_alg.cpp src/adc_nav_client_alg_node.cpp)
-
-target_link_libraries(${client_name} ${catkin_LIBRARIES})
-target_link_libraries(${client_name} ${iriutils_LIBRARY})
-target_link_libraries(${client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
-
-add_dependencies(${client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name})
-add_dependencies(${client_name} ${adc_nav_client_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-#BT_module
-#add_library(${module_bt_name} src/adc_nav_bt_module.cpp)
-#target_link_libraries(${module_bt_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
-#target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
-#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})
#############
## Install ##
diff --git a/cfg/ADCNavBtClient.cfg b/cfg/ADCNavBtClient.cfg
deleted file mode 100755
index 5c7ad97..0000000
--- a/cfg/ADCNavBtClient.cfg
+++ /dev/null
@@ -1,56 +0,0 @@
-#! /usr/bin/env python
-#* All rights reserved.
-#*
-#* Redistribution and use in source and binary forms, with or without
-#* modification, are permitted provided that the following conditions
-#* are met:
-#*
-#* * Redistributions of source code must retain the above copyright
-#* notice, this list of conditions and the following disclaimer.
-#* * Redistributions in binary form must reproduce the above
-#* copyright notice, this list of conditions and the following
-#* disclaimer in the documentation and/or other materials provided
-#* with the distribution.
-#* * Neither the name of the Willow Garage nor the names of its
-#* contributors may be used to endorse or promote products derived
-#* from this software without specific prior written permission.
-#*
-#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-#* POSSIBILITY OF SUCH DAMAGE.
-#***********************************************************
-
-# Author:
-
-PACKAGE='adc_nav_module'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-from iri_base_bt_client.submodule import add_bt_client_params
-
-gen = ParameterGenerator()
-move_base = gen.add_group("Move base action")
-
-# 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_bt_client_params(gen)
-gen.add("START", bool_t, 0, "synchronous start", False)
-gen.add("RESTART", bool_t, 0, "asynchronous restart of BT", False)
-
-move_base.add("set_go_to_input", bool_t, 0, "Set go to parameters", False)
-move_base.add("mb_x_pos", double_t, 0, "Target X position", 0.0, -100.0, 100.0)
-move_base.add("mb_y_pos", double_t, 0, "Target Y position", 0.0, -100.0, 100.0)
-move_base.add("mb_xy_tol", double_t, 0, "Target XY tolerance", -1.0, -1.0, 10.0)
-move_base.add("mb_yaw", double_t, 0, "Target Yaw angle", 0.0, -3.14159,3.14159)
-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")
-
-exit(gen.generate(PACKAGE, "ADCNavModuleBtClientAlgNode", "ADCNavBtClient"))
diff --git a/cfg/ADCNavClient.cfg b/cfg/ADCNavClient.cfg
deleted file mode 100755
index a39277e..0000000
--- a/cfg/ADCNavClient.cfg
+++ /dev/null
@@ -1,68 +0,0 @@
-#! /usr/bin/env python
-#* All rights reserved.
-#*
-#* Redistribution and use in source and binary forms, with or without
-#* modification, are permitted provided that the following conditions
-#* are met:
-#*
-#* * Redistributions of source code must retain the above copyright
-#* notice, this list of conditions and the following disclaimer.
-#* * Redistributions in binary form must reproduce the above
-#* copyright notice, this list of conditions and the following
-#* disclaimer in the documentation and/or other materials provided
-#* with the distribution.
-#* * Neither the name of the Willow Garage nor the names of its
-#* contributors may be used to endorse or promote products derived
-#* from this software without specific prior written permission.
-#*
-#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-#* POSSIBILITY OF SUCH DAMAGE.
-#***********************************************************
-
-# Author:
-
-PACKAGE='iri_adc_nav_module'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-
-gen = ParameterGenerator()
-
-move_base = gen.add_group("Move base action")
-costmaps = gen.add_group("Costmap")
-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)
-move_base.add("mb_x_pos", double_t, 0, "Target X position", 0.0, -100.0, 100.0)
-move_base.add("mb_y_pos", double_t, 0, "Target Y position", 0.0, -100.0, 100.0)
-move_base.add("mb_xy_tol", double_t, 0, "Target XY tolerance", -1.0, -1.0, 10.0)
-move_base.add("mb_yaw", double_t, 0, "Target Yaw angle", 0.0, -3.14159,3.14159)
-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")
-
-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)
-costmaps.add("cm_clear_costmaps", bool_t, 0, "Clear costmaps", False)
-
-move_base.add("start_pose_nav", bool_t, 0, "Start navigating towards a pose", False)
-move_base.add("start_position_nav",bool_t, 0, "Start navigating towards a position", False)
-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/ADCNavModule.cfg b/cfg/ADCNavModule.cfg
deleted file mode 100755
index 74d6be7..0000000
--- a/cfg/ADCNavModule.cfg
+++ /dev/null
@@ -1,44 +0,0 @@
-#! /usr/bin/env python
-#* All rights reserved.
-#*
-#* Redistribution and use in source and binary forms, with or without
-#* modification, are permitted provided that the following conditions
-#* are met:
-#*
-#* * Redistributions of source code must retain the above copyright
-#* notice, this list of conditions and the following disclaimer.
-#* * Redistributions in binary form must reproduce the above
-#* copyright notice, this list of conditions and the following
-#* disclaimer in the documentation and/or other materials provided
-#* with the distribution.
-#* * Neither the name of the Willow Garage nor the names of its
-#* contributors may be used to endorse or promote products derived
-#* from this software without specific prior written permission.
-#*
-#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-#* POSSIBILITY OF SUCH DAMAGE.
-#***********************************************************
-
-# Author:
-
-PACKAGE='iri_adc_nav_module'
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-from iri_adc_nav_module.dyn_params import add_nav_module_params
-
-gen = ParameterGenerator()
-
-nav_module = add_nav_module_params(gen,"nav")
-
-exit(gen.generate(PACKAGE, "ADCNavModuleAlgorithm", "ADCNavModule"))
-
diff --git a/config/adc_nav_module_default.yaml b/config/adc_nav_module_default.yaml
deleted file mode 100644
index 20f5b03..0000000
--- a/config/adc_nav_module_default.yaml
+++ /dev/null
@@ -1,24 +0,0 @@
-nav_module_rate_hz: 1.0
-
-move_base_max_retries: 1
-move_base_feedback_watchdog_time_s: 1.0
-move_base_enable_watchdog: True
-move_base_timeout_s: 100.0
-move_base_enable_timeout: True
-move_base_frame_id: "map"
-move_base_enabled: True
-move_base_cancel_prev: True
-
-odom_watchdog_time_s: 1.0
-
-tf_timeout_time_s: 5.0
-
-clear_costmaps_max_retries: 1.0
-clear_costmaps_enable_auto_clear: False
-clear_costmaps_auto_clear_rate_hz: 0.1
-clear_costmaps_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_bt_client_alg_node.h b/include/adc_nav_bt_client_alg_node.h
deleted file mode 100644
index 2a5f098..0000000
--- a/include/adc_nav_bt_client_alg_node.h
+++ /dev/null
@@ -1,159 +0,0 @@
-// Copyright (C) Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author
-// All rights reserved.
-//
-// This file is part of iri-ros-pkg
-// iri-ros-pkg is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-// IMPORTANT NOTE: This code has been generated through a script from the
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
-// of the scripts. ROS topics can be easly add by using those scripts. Please
-// refer to the IRI wiki page for more information:
-// http://wikiri.upc.es/index.php/Robotics_Lab
-
-#ifndef _adc_nav_module_bt_client_alg_node_h_
-#define _adc_nav_module_bt_client_alg_node_h_
-
-#include <iri_base_bt_client/iri_base_bt_client.h>
-#include <adc_nav_module/ADCNavBtClientConfig.h>
-
-#include "adc_nav_module/adc_nav_bt_module.h"
-#include <sensor_msgs/JointState.h>
-#include <geometry_msgs/PoseStamped.h>
-
-// [publisher subscriber headers]
-
-// [service client headers]
-
-// [action server client headers]
-
-/**
- * \brief IRI ROS Specific Algorithm Class
- *
- */
-class ADCNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<adc_nav_module::ADCNavBtClientConfig>
-{
- private:
- // object of module
- CADCNavModuleBT adc_nav_module_bt;
-
- // Client actions and conditions (always return BT::NodeStatus::SUCCESS)
-
- // [publisher attributes]
-
- // [subscriber attributes]
-
- // [service attributes]
-
- // [client attributes]
-
- // [action server attributes]
-
- // [action client attributes]
-
- bool set_go_to_pose;
- /**
- * \brief Async function to set the adc_nav_move_to_joints input ports.
- *
- * It takes the values from Dynamic Reconfigure.
- *
- * It has the following output ports:
- *
- * x (double): desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * y (double): desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * yaw (double): desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * heading_tol (double): heading tolerance for the goal.
- *
- * x_y_pos_tol (double): position tolerance for the goal.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus set_adc_nav_go_to_pose_inputs(BT::TreeNode& self);
-
- /**
- * \brief Async function to set the set_adc_nav_goal_frame input ports
- *
- * It has the following output ports:
- *
- * frame_id (std::string): name of the new reference frame for the future goals.
- * This frame can be any that exist inside the TF tree
- * of the robot.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus set_adc_nav_goal_frame_inputs(BT::TreeNode& self);
-
- public:
- /**
- * \brief Constructor
- *
- * This constructor initializes specific class attributes and all ROS
- * communications variables to enable message exchange.
- */
- ADCNavModuleBtClientAlgNode(void);
-
- /**
- * \brief Destructor
- *
- * This destructor frees all necessary dynamic memory allocated within this
- * this class.
- */
- ~ADCNavModuleBtClientAlgNode(void);
-
- protected:
-
- /**
- * \brief dynamic reconfigure server callback
- *
- * This method is called whenever a new configuration is received through
- * the dynamic reconfigure. The derivated generic algorithm class must
- * implement it.
- *
- * \param config an object with new configuration from all algorithm
- * parameters defined in the config file.
- * \param level integer referring the level in which the configuration
- * has been changed.
- */
- void node_config_update(adc_nav_module::ADCNavBtClientConfig &config, uint32_t level);
-
- /**
- * \brief node add diagnostics
- *
- * In this abstract function additional ROS diagnostics applied to the
- * specific algorithms may be added.
- */
- void addNodeDiagnostics(void);
-
- // [diagnostic functions]
-
- // [test functions]
-};
-
-#endif
diff --git a/include/adc_nav_client_alg.h b/include/adc_nav_client_alg.h
deleted file mode 100644
index eda4697..0000000
--- a/include/adc_nav_client_alg.h
+++ /dev/null
@@ -1,131 +0,0 @@
-// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author
-// All rights reserved.
-//
-// This file is part of iri-ros-pkg
-// iri-ros-pkg is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-// IMPORTANT NOTE: This code has been generated through a script from the
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
-// of the scripts. ROS topics can be easly add by using those scripts. Please
-// refer to the IRI wiki page for more information:
-// http://wikiri.upc.es/index.php/Robotics_Lab
-
-#ifndef _adc_nav_client_alg_h_
-#define _adc_nav_client_alg_h_
-
-#include <iri_adc_nav_module/ADCNavClientConfig.h>
-
-//include humanoid_common_adc_nav_client_alg main library
-
-/**
- * \brief IRI ROS Specific Driver Class
- *
- *
- */
-class ADCNavClientAlgorithm
-{
- protected:
- /**
- * \brief define config type
- *
- * Define a Config type with the ADCNavClientConfig. All driver implementations
- * will then use the same variable type Config.
- */
- pthread_mutex_t access_;
-
- // private attributes and methods
-
- public:
- /**
- * \brief define config type
- *
- * Define a Config type with the ADCNavClientConfig. All driver implementations
- * will then use the same variable type Config.
- */
- typedef iri_adc_nav_module::ADCNavClientConfig Config;
-
- /**
- * \brief config variable
- *
- * This variable has all the driver parameters defined in the cfg config file.
- * Is updated everytime function config_update() is called.
- */
- Config config_;
-
- /**
- * \brief constructor
- *
- * In this constructor parameters related to the specific driver can be
- * initalized. Those parameters can be also set in the openDriver() function.
- * Attributes from the main node driver class IriBaseDriver such as loop_rate,
- * may be also overload here.
- */
- ADCNavClientAlgorithm(void);
-
- /**
- * \brief Lock Algorithm
- *
- * Locks access to the Algorithm class
- */
- void lock(void) { pthread_mutex_lock(&this->access_); };
-
- /**
- * \brief Unlock Algorithm
- *
- * Unlocks access to the Algorithm class
- */
- void unlock(void) { pthread_mutex_unlock(&this->access_); };
-
- /**
- * \brief Tries Access to Algorithm
- *
- * Tries access to Algorithm
- *
- * \return true if the lock was adquired, false otherwise
- */
- bool try_enter(void)
- {
- if(pthread_mutex_trylock(&this->access_)==0)
- return true;
- else
- return false;
- };
-
- /**
- * \brief config update
- *
- * In this function the driver parameters must be updated with the input
- * config variable. Then the new configuration state will be stored in the
- * Config attribute.
- *
- * \param new_cfg the new driver configuration state
- *
- * \param level level in which the update is taken place
- */
- void config_update(Config& new_cfg, uint32_t level=0);
-
- // here define all adc_nav_client_alg interface methods to retrieve and set
- // the driver parameters
-
- /**
- * \brief Destructor
- *
- * This destructor is called when the object is about to be destroyed.
- *
- */
- ~ADCNavClientAlgorithm(void);
-};
-
-#endif
diff --git a/include/adc_nav_client_alg_node.h b/include/adc_nav_client_alg_node.h
deleted file mode 100644
index 1326e32..0000000
--- a/include/adc_nav_client_alg_node.h
+++ /dev/null
@@ -1,119 +0,0 @@
-// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
-// Author
-// All rights reserved.
-//
-// This file is part of iri-ros-pkg
-// iri-ros-pkg is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-// IMPORTANT NOTE: This code has been generated through a script from the
-// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
-// of the scripts. ROS topics can be easly add by using those scripts. Please
-// refer to the IRI wiki page for more information:
-// http://wikiri.upc.es/index.php/Robotics_Lab
-
-#ifndef _adc_nav_client_alg_node_h_
-#define _adc_nav_client_alg_node_h_
-
-#include <iri_base_algorithm/iri_base_algorithm.h>
-#include "adc_nav_client_alg.h"
-
-// [publisher subscriber headers]
-
-// [service client headers]
-
-// [nav server client headers]
-
-#include <iri_adc_nav_module/adc_nav_module.h>
-
-/**
- * \brief IRI ROS Specific Algorithm Class
- *
- */
-class ADCNavClientAlgNode : public algorithm_base::IriBaseAlgorithm<ADCNavClientAlgorithm>
-{
- private:
- // [publisher attributes]
-
- // [subscriber attributes]
-
- // [service attributes]
-
- // [client attributes]
-
- // [nav server attributes]
-
- // [nav client attributes]
-
- CADCNavModule nav;
-
- public:
- /**
- * \brief Constructor
- *
- * This constructor initializes specific class attributes and all ROS
- * communications variables to enable message exchange.
- */
- ADCNavClientAlgNode(void);
-
- /**
- * \brief Destructor
- *
- * This destructor frees all necessary dynamic memory allocated within this
- * this class.
- */
- ~ADCNavClientAlgNode(void);
-
- protected:
- /**
- * \brief main node thread
- *
- * This is the main thread node function. Code written here will be executed
- * in every node loop while the algorithm is on running state. Loop frequency
- * can be tuned by modifying loop_rate attribute.
- *
- * Here data related to the process loop or to ROS topics (mainly data structs
- * related to the MSG and SRV files) must be updated. ROS publisher objects
- * must publish their data in this process. ROS client servers may also
- * request data to the corresponding server topics.
- */
- void mainNodeThread(void);
-
- /**
- * \brief dynamic reconfigure server callback
- *
- * This method is called whenever a new configuration is received through
- * the dynamic reconfigure. The derivated generic algorithm class must
- * implement it.
- *
- * \param config an object with new configuration from all algorithm
- * parameters defined in the config file.
- * \param level integer referring the level in which the configuration
- * has been changed.
- */
- void node_config_update(Config &config, uint32_t level);
-
- /**
- * \brief node add diagnostics
- *
- * In this abstract function additional ROS diagnostics applied to the
- * specific algorithms may be added.
- */
- void addNodeDiagnostics(void);
-
- // [diagnostic functions]
-
- // [test functions]
-};
-
-#endif
diff --git a/include/iri_adc_nav_module/adc_nav_bt_module.h b/include/iri_adc_nav_module/adc_nav_bt_module.h
deleted file mode 100644
index 9ffbfbb..0000000
--- a/include/iri_adc_nav_module/adc_nav_bt_module.h
+++ /dev/null
@@ -1,650 +0,0 @@
-#ifndef _IRI_ADC_NAV_MODULE_BT_H
-#define _IRI_ADC_NAV_MODULE_BT_H
-
-#include <iri_base_algorithm/iri_base_algorithm.h>
-
-#include "iri_bt_factory.h"
-#include <adc_nav_module/adc_nav_module.h>
-
-class CADCNavModuleBT
-{
- private:
- // object of adc_nav_module
- CADCNavModule adc_nav_module;
-
- public:
- /**
- * \brief Constructor
- *
- */
- CADCNavModuleBT();
-
- /**
- * \brief Constructor with module name.
- *
- * \param module_name The module name.
- *
- */
- CADCNavModuleBT(std::string &module_name);
-
- /**
- * \brief Register all conditions and actions needed for using the module
- *
- * This function registers all conditions and actions needed for using the module
- * to the factory provided including all input and output ports required
- * by actions.
- *
- * \param factory (IriBehaviorTreeFactory)
- *
- */
- void init(IriBehaviorTreeFactory &factory);
-
- /**
- * \brief Destructor
- *
- * This destructor frees all necessary dynamic memory allocated within this
- * this class.
- */
- ~CADCNavModuleBT();
-
- protected:
-
- /**
- * \brief Synchronized go_to_pose adc nav function.
- *
- * This function calls go_to_pose of adc_nav_module. As this is a
- * synchronous action is_adc_nav_finished() must be used to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * x (double): desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * y (double): desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * yaw (double): desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * heading_tol (double): [Optional] heading tolerance for the goal.
- *
- * x_y_pos_tol (double): [Optional] position tolerance for the goal.
- *
- * \param self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus sync_adc_nav_go_to_pose(BT::TreeNode& self);
-
- /**
- * \brief Async go_to_pose adc nav function.
- *
- * This function calls go_to_pose of adc_nav_module. As this is
- * an asynchronous action is_adc_nav_finished() is checked to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * x (double): desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * y (double): desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * yaw (double): desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * heading_tol (double): [Optional] heading tolerance for the goal.
- *
- * x_y_pos_tol (double): [Optional] position tolerance for the goal.
- *
- * It also has a bidirectional port:
- *
- * new_goal (bool): If it's a new_goal.
- *
- * \param self node with the required inputs defined as follows:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
- * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
- * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
- * it also returns BT:NodeStatus::FAILURE
- *
- */
- BT::NodeStatus async_adc_nav_go_to_pose(BT::TreeNode& self);
-
- /**
- * \brief Synchronized go_to_orientation adc nav function.
- *
- * This function calls go_to_orientation of adc_nav_module. As this is a
- * synchronous action is_adc_nav_finished() must be used to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * yaw (double): desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * heading_tol (double): [Optional] heading tolerance for the goal.
- *
- * \param self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus sync_adc_nav_go_to_orientation(BT::TreeNode& self);
-
- /**
- * \brief Async go_to_orientation adc nav function.
- *
- * This function calls go_to_orientation of adc_nav_module. As this is
- * an asynchronous action is_adc_nav_finished() is checked to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * yaw (double): desired orientation with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * heading_tol (double): [Optional] heading tolerance for the goal.
- *
- * It also has a bidirectional port:
- *
- * new_goal (bool): If it's a new_goal.
- *
- * \param self node with the required inputs defined as follows:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
- * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
- * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
- * it also returns BT:NodeStatus::FAILURE
- *
- */
- BT::NodeStatus async_adc_nav_go_to_orientation(BT::TreeNode& self);
-
- /**
- * \brief Synchronized go_to_position adc nav function.
- *
- * This function calls go_to_position of adc_nav_module. As this is a
- * synchronous action is_adc_nav_finished() must be used to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * x (double): desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * y (double): desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * x_y_pos_tol (double): [Optional] position tolerance for the goal.
- *
- * \param self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus sync_adc_nav_go_to_position(BT::TreeNode& self);
-
- /**
- * \brief Async go_to_position adc nav function.
- *
- * This function calls go_to_position of adc_nav_module. As this is
- * an asynchronous action is_adc_nav_finished() is checked to know when the action
- * has actually finished.
- *
- * It has the following input ports:
- *
- * x (double): desired X positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * y (double): desired Y positon with respect to the goal frame set by the
- * set_goal_frame() function.
- *
- * x_y_pos_tol (double): [Optional] position tolerance for the goal.
- *
- * It also has a bidirectional port:
- *
- * new_goal (bool): If it's a new_goal.
- *
- * \param self node with the required inputs defined as follows:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
- * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
- * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
- * it also returns BT:NodeStatus::FAILURE
- *
- */
- BT::NodeStatus async_adc_nav_go_to_position(BT::TreeNode& self);
-
- /**
- * \brief Synchronized set_adc_nav_goal_frame adc nav function.
- *
- * This function calls set_adc_nav_goal_frame of adc_nav_module.
- *
- * It has the following input ports:
- *
- * frame_id (std::string): name of the new reference frame for the future goals.
- * This frame can be any that exist inside the TF tree
- * of the robot.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus set_adc_nav_goal_frame(BT::TreeNode& self);
-
- /**
- * \brief Synchronized costmaps_clear adc nav function.
- *
- * This function calls costmaps_clear of adc_nav_module.
- *
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_costmaps_clear(void);
-
- /**
- * \brief Synchronized costmaps_enable_auto_clear adc nav function.
- *
- * This function calls costmaps_enable_auto_clear of adc_nav_module.
- *
- * It has the following input ports:
- *
- * rate_hz (double): 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.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_costmaps_enable_auto_clear(BT::TreeNode& self);
-
- /**
- * \brief Synchronized costmaps_disable_auto_clear adc nav function.
- *
- * This function calls costmaps_disable_auto_clear of adc_nav_module.
- *
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_costmaps_disable_auto_clear(void);
-
- /**
- * \brief Synchronized set_parameter adc nav function.
- *
- * This function calls set_parameter of adc_nav_module.
- *
- * It has the following input ports:
- *
- * name_space (std::string): String with the parameter name space.
- *
- * name (std::string): String with the name of the parameter to change. This must
- * coincide with a parameter name on the associated dynamic
- * reconfigure server.
- *
- * value (bool): the new value for the parameter.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_set_bool_parameter(BT::TreeNode& self);
-
- /**
- * \brief Synchronized set_parameter adc nav function.
- *
- * This function calls set_parameter of adc_nav_module.
- *
- * It has the following input ports:
- *
- * name_space (std::string): String with the parameter name space.
- *
- * name (std::string): String with the name of the parameter to change. This must
- * coincide with a parameter name on the associated dynamic
- * reconfigure server.
- *
- * value (int): the new value for the parameter.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_set_int_parameter(BT::TreeNode& self);
-
- /**
- * \brief Synchronized set_parameter adc nav function.
- *
- * This function calls set_parameter of adc_nav_module.
- *
- * It has the following input ports:
- *
- * name_space (std::string): String with the parameter name space.
- *
- * name (std::string): String with the name of the parameter to change. This must
- * coincide with a parameter name on the associated dynamic
- * reconfigure server.
- *
- * value (std::string): the new value for the parameter.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_set_string_parameter(BT::TreeNode& self);
-
- /**
- * \brief Synchronized set_parameter adc nav function.
- *
- * This function calls set_parameter of adc_nav_module.
- *
- * It has the following input ports:
- *
- * name_space (std::string): String with the parameter name space.
- *
- * name (std::string): String with the name of the parameter to change. This must
- * coincide with a parameter name on the associated dynamic
- * reconfigure server.
- *
- * value (double): the new value for the parameter.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus adc_nav_set_double_parameter(BT::TreeNode& self);
-
- /**
- * \brief Synchronized getCurrentPose adc nav function.
- *
- * This function calls getCurrentPose of adc_nav_module.
- *
- * It has the following output ports:
- *
- * pose (geometry_msgs::Pose): current pose.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus get_adc_nav_current_pose(BT::TreeNode& self);
-
- /**
- * \brief Synchronized getGoalDistance adc nav function.
- *
- * This function calls getGoalDistance of adc_nav_module.
- *
- * It has the following output ports:
- *
- * distance (double): current distance to goal.
- *
- * \param self Self node with the required ports:
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus get_adc_nav_goal_distance(BT::TreeNode& self);
-
- /**
- * \brief Synchronized stop adc nav function.
- *
- * This function calls stop of adc_nav_module. As this is a
- * synchronous action is_adc_nav_finished() must be used to know when the action
- * has actually finished.
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
- *
- */
- BT::NodeStatus sync_cancel_adc_nav_action(void);
-
- /**
- * \brief Async stop adc nav function.
- *
- * This function calls stop of adc_nav_module. As this is
- * an asynchronous action is_adc_nav_finished() is checked to know when the action
- * has actually finished.
- *
- * \return a BT:NodeStatus indicating whether the request has been
- * successfull and the action has finished (BT:NodeStatus::SUCCESS) or the
- * request has been unsuccessfull (BT:NodeStatus::FAILURE) or it is still
- * in progress (BT:NodeStatus::RUNNING). If inputs are missing or incorrect
- * it also returns BT:NodeStatus::FAILURE
- *
- */
- BT::NodeStatus async_cancel_adc_nav_action(void);
-
- /**
- * \brief Checks if the module is idle and there isn't any new movement request.
- *
- * This function must be used when any sync action is called.
- *
- * \return a BT:NodeStatus indicating whether the last movement has
- * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE),
- * regardless of its success or not.
- *
- */
- BT::NodeStatus is_adc_nav_finished(void);
-
- /**
- * \brief Checks if the module is idle and there isn't any new movement request.
- *
- * This function must be used when any sync action is called and
- * a BT timeout is used.
- *
- * \return a BT:NodeStatus indicating whether the last movement has
- * ended (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::RUNNING),
- * regardless of its success or not.
- *
- */
- BT::NodeStatus async_is_adc_nav_finished(void);
-
- ///adc nav module status
- /**
- * \brief Checks if the module is active and operating properly.
- *
- * This function must be used to know whether the module is active and
- * operating properly, or not.
- *
- * \return a BT:NodeStatus indicating whether the module is active and
- * operating properly (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_running(void);
-
- /**
- * \brief Checks if the last action goal has been reached successfully.
- *
- * This function must be used to know whether the last action goal has
- * been reached successfully or not, once it has finished.
- *
- * \return a BT:NodeStatus indicating whether the last action goal has
- * been reached successfully (BT:NodeStatus::SUCCESS) or not
- * (BT:NodeStatus::FAILURE), regardless of how it has failed.
- *
- */
- BT::NodeStatus is_adc_nav_module_success(void);
-
- /**
- * \brief Checks if the goal could not be started.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * goal could not be started.
- *
- * \return a BT:NodeStatus indicating whether the goal could not be started
- * (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_action_start_fail(void);
-
- /**
- * \brief Checks if the goal did not complete in the allowed time.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * goal did not complete in the allowed time.
- *
- * \return a BT:NodeStatus indicating whether the goal did not complete in
- * the allowed time (BT:NodeStatus::SUCCESS) or if it failed due to another
- * reason (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_timeout(void);
-
- /**
- * \brief Checks if the feedback has not been received for a long time.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * feedback has not been received for a long time.
- *
- * \return a BT:NodeStatus indicating whether the feedback has not been
- * received for a long time (BT:NodeStatus::SUCCESS) or if it failed due
- * to another reason (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_fb_watchdog(void);
-
- /**
- * \brief Checks if the action goal could not be reached.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * action goal could not be reached.
- *
- * \return a BT:NodeStatus indicating whether the action goal could not
- * be reached (BT:NodeStatus::SUCCESS) or if it failed due to another
- * reason (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_aborted(void);
-
- /**
- * \brief Checks if the current action goal has been cancelled by
- * another goal.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * current action goal has been cancelled by another goal.
- *
- * \return a BT:NodeStatus indicating whether the current action goal
- * has been cancelled by another goal (BT:NodeStatus::SUCCESS) or if it
- * failed due to another reason (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_preempted(void);
-
- /**
- * \brief Checks if the goal information is not valid.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * goal information is not valid and the goal will not be executed.
- *
- * \return a BT:NodeStatus indicating whether the goal information is not
- * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_rejected(void);
-
- /**
- * \brief Checks if set_param function has failed.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because
- * set_param function has failed.
- *
- * \return a BT:NodeStatus indicating whether the goal information is not
- * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_set_param_fail(void);
-
- /**
- * \brief Checks if the requested parameter is not present.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * requested parameter is not present.
- *
- * \return a BT:NodeStatus indicating whether the goal information is not
- * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_param_not_present(void);
-
- /**
- * \brief Checks if no odom messages have been received on a while.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because
- * no odom messages have been received on a while.
- *
- * \return a BT:NodeStatus indicating whether the goal information is not
- * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_no_odom(void);
-
- /**
- * \brief Checks if the requested transform does not exist.
- *
- * This function must be used to know the type of failure, once the last
- * goal has finished. Specifically, it checks if it failed because the
- * requested transform does not exist.
- *
- * \return a BT:NodeStatus indicating whether the goal information is not
- * valid (BT:NodeStatus::SUCCESS) or if it failed due to another reason
- * (BT:NodeStatus::FAILURE).
- *
- */
- BT::NodeStatus is_adc_nav_module_no_transform(void);
- };
-
- #endif
diff --git a/include/iri_adc_nav_module/adc_nav_module.h b/include/iri_adc_nav_module/adc_nav_module.h
deleted file mode 100644
index 032a20b..0000000
--- a/include/iri_adc_nav_module/adc_nav_module.h
+++ /dev/null
@@ -1,262 +0,0 @@
-#ifndef _ADC_NAV_MODULE_H
-#define _ADC_NAV_MODULE_H
-
-// IRI ROS headers
-#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 <iri_adc_nav_module/ADCNavModuleConfig.h>
-
-//States
-typedef enum {ADC_NAV_MODULE_IDLE,
- ADC_NAV_MODULE_TRANSFORM,
- ADC_NAV_MODULE_START,
- ADC_NAV_MODULE_WAIT} adc_nav_module_state_t;
-
-//Status
-/**
- * \brief Possible module status returned by the get_status() function
- *
- * These are the possible status of the module, which are:
- * * ADC_NAV_MODULE_RUNNING: indicates that the module is active and operating
- * properly.
- * * ADC_NAV_MODULE_SUCCESS: indicates the last navigation goal has been reached
- * successfully.
- * * ADC_NAV_MODULE_ACTION_START_FAIL: indicates that the goal could not be started.
- * * ADC_NAV_MODULE_TIMEOUT: indicates that the goal did not complete in the
- * allowed time.
- * * ADC_NAV_MODULE_FB_WATCHDOG: indicates that feedback has not been received for
- * a long time.
- * * ADC_NAV_MODULE_ABORTED: indicates that the navigation goal could not be reached.
- * * ADC_NAV_MODULE_PREEMPTED: indicates that the current navigation goal has been
- * canceled by another goal.
- * * ADC_NAV_MODULE_REJECTED: indicates that the goal informatio is not valid and the
- * goal will not be executed.
- * * ADC_NAV_MODULE_SET_PARAM_FAIL: indicates that it was impossible to set the value
- * of a parameter.
- * * ADC_NAV_MODULE_PARAM_NOT_PRESENT: indicates that the parameter to be changed is
- * not present in the remote node.
- * * ADC_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.
- * * ADC_NAV_MODULE_NO_TRANSFORM: indicates that there exist no transform between the
- * desired goal frame and the rest of the TF tree.
- */
-typedef enum {ADC_NAV_MODULE_RUNNING,
- ADC_NAV_MODULE_SUCCESS,
- ADC_NAV_MODULE_ACTION_START_FAIL,
- ADC_NAV_MODULE_TIMEOUT,
- ADC_NAV_MODULE_FB_WATCHDOG,
- ADC_NAV_MODULE_ABORTED,
- ADC_NAV_MODULE_PREEMPTED,
- ADC_NAV_MODULE_REJECTED,
- ADC_NAV_MODULE_SET_PARAM_FAIL,
- ADC_NAV_MODULE_PARAM_NOT_PRESENT,
- ADC_NAV_MODULE_NO_ODOM,
- ADC_NAV_MODULE_NO_TRANSFORM} adc_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
-
-/**
- * \brief
- *
- */
-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
- *
- */
- adc_nav_module_state_t state;
- /**
- * \brief
- *
- */
- adc_nav_module_status_t status;
- /**
- * \brief
- *
- */
- bool new_goal;
- /**
- * \brief
- *
- */
- double goal_x;
- /**
- * \brief
- *
- */
- double goal_y;
- /**
- * \brief
- *
- */
- double goal_yaw;
- /**
- * \brief
- *
- */
- bool cancel_pending;
-
- protected:
- /**
- * \brief
- *
- */
- void state_machine(void);
- /**
- * \brief
- *
- */
- void reconfigure_callback(iri_adc_nav_module::ADCNavModuleConfig &config, uint32_t level);
- public:
- /**
- * \brief Constructor
- *
- */
- CADCNavModule(const std::string &name,const std::string &name_space=std::string(""));
- // basic navigation functions
- /**
- * \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.
- *
- * \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);
- /**
- * \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.
- *
- * \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);
- /**
- * \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.
- *
- * \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.
- *
- * \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);
- /**
- * \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 adc_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.
- */
- adc_nav_module_status_t get_status(void);
- /**
- * \brief Destructor
- *
- */
- ~CADCNavModule();
- };
-
- #endif
diff --git a/include/iri_adc_nav_module/ackermann_lp_module.h b/include/iri_nav_module/ackermann_lp_module.h
similarity index 99%
rename from include/iri_adc_nav_module/ackermann_lp_module.h
rename to include/iri_nav_module/ackermann_lp_module.h
index 4078439..ec50306 100644
--- a/include/iri_adc_nav_module/ackermann_lp_module.h
+++ b/include/iri_nav_module/ackermann_lp_module.h
@@ -2,7 +2,7 @@
#define _ACKERMANN_LP_MODULE_H
// IRI ROS headers
-#include <iri_adc_nav_module/nav_planner_module.h>
+#include <iri_nav_module/nav_planner_module.h>
/**
* \brief
diff --git a/include/iri_adc_nav_module/nav_costmap_module.h b/include/iri_nav_module/nav_costmap_module.h
similarity index 90%
rename from include/iri_adc_nav_module/nav_costmap_module.h
rename to include/iri_nav_module/nav_costmap_module.h
index 5a54854..36bea84 100644
--- a/include/iri_adc_nav_module/nav_costmap_module.h
+++ b/include/iri_nav_module/nav_costmap_module.h
@@ -2,6 +2,7 @@
#define _NAV_COSTMAP_MODULE_H
#include <geometry_msgs/Point.h>
+#include <costmap_2d/footprint.h>
// IRI ROS headers
#include <iri_ros_tools/module_dyn_reconf.h>
@@ -167,13 +168,34 @@ class CNavCostmapModule
template <class ModuleCfg>
dyn_reconf_status_t CNavCostmapModule<ModuleCfg>::get_footprint(std::vector<geometry_msgs::Point> &footprint)
{
+ std::string footprint_string;
+ if(this->costmap_reconf.get_parameter("footprint",footprint_string))
+ {
+ if(costmap_2d::makeFootprintFromString(footprint_string,footprint))
+ 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_footprint(std::vector<geometry_msgs::Point> &footprint)
{
+ std::stringstream footprint_string;
+ footprint_string << "[";
+ for(unsigned int i=0;i<footprint.size();i++)
+ {
+ footprint_string << "[" << footprint[i].x << "," << footprint[i].y << "]";
+ if(i<footprint.size()-1)
+ std::cout << ",";
+ }
+ footprint_string << "]";
+ this->costmap_reconf.set_parameter("footprint",footprint_string.str());
+ return this->costmap_reconf.get_status();
}
template <class ModuleCfg>
diff --git a/include/iri_adc_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
similarity index 99%
rename from include/iri_adc_nav_module/nav_module.h
rename to include/iri_nav_module/nav_module.h
index 40c2404..02f3732 100644
--- a/include/iri_adc_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -20,7 +20,7 @@
#include <move_base_msgs/MoveBaseAction.h>
#include <std_srvs/Empty.h>
-#include <iri_adc_nav_module/nav_costmap_module.h>
+#include <iri_nav_module/nav_costmap_module.h>
/**
* \brief Navigation module
diff --git a/include/iri_adc_nav_module/nav_planner_module.h b/include/iri_nav_module/nav_planner_module.h
similarity index 100%
rename from include/iri_adc_nav_module/nav_planner_module.h
rename to include/iri_nav_module/nav_planner_module.h
diff --git a/include/iri_adc_nav_module/opendrive_gp_module.h b/include/iri_nav_module/opendrive_gp_module.h
similarity index 99%
rename from include/iri_adc_nav_module/opendrive_gp_module.h
rename to include/iri_nav_module/opendrive_gp_module.h
index b161d82..bb4d4ee 100644
--- a/include/iri_adc_nav_module/opendrive_gp_module.h
+++ b/include/iri_nav_module/opendrive_gp_module.h
@@ -3,7 +3,7 @@
// IRI ROS headers
#include <iri_ros_tools/module_service.h>
-#include <iri_adc_nav_module/nav_planner_module.h>
+#include <iri_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>
diff --git a/launch/adc_nav_bt_client.launch b/launch/adc_nav_bt_client.launch
deleted file mode 100644
index 711ddb6..0000000
--- a/launch/adc_nav_bt_client.launch
+++ /dev/null
@@ -1,57 +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" />
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_nav_module"
- type="tiago_nav_bt_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/tiago_nav_module/move_base"
- to="$(arg move_base_action)"/>
- <remap from="~/tiago_nav_module/move_poi"
- to="$(arg move_poi_action)"/>
- <remap from="~/tiago_nav_module/move_waypoint"
- to="$(arg move_waypoint_action)"/>
- <remap from="~/tiago_nav_module/odom"
- to="$(arg odom_topic)"/>
- <remap from="~/tiago_nav_module/path"
- to="$(arg path_topic)"/>
- <remap from="~/tiago_nav_module/clear_costmaps"
- to="$(arg clear_costmaps_service)"/>
- <remap from="~/tiago_nav_module/change_map"
- to="$(arg change_map_service)"/>
- <remap from="~/tiago_nav_module/set_op_mode"
- to="$(arg set_op_mode_service)"/>
- <remap from="~/tiago_nav_module/move_base_reconf"
- to="$(arg move_base_dyn_reconf)"/>
-
- <rosparam file="$(arg config_file)" command="load" ns="tiago_nav_module" />
- <param name="path" value="$(arg tree_path)"/>
- <param name="tree_xml_file" value="$(arg tree_file)"/>
- <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/adc_nav_client.launch b/launch/adc_nav_client.launch
deleted file mode 100644
index 1e196dc..0000000
--- a/launch/adc_nav_client.launch
+++ /dev/null
@@ -1,59 +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 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/package.xml b/package.xml
index 16998b7..7278ca1 100644
--- a/package.xml
+++ b/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package>
- <name>iri_adc_nav_module</name>
+ <name>iri_nav_module</name>
<version>0.0.0</version>
- <description>Module iri_adc_nav_module package</description>
+ <description>Module iri_nav_module package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
@@ -49,9 +49,8 @@
<build_depend>nav_msgs</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>
<build_depend>iri_adc_msgs</build_depend>
+ <build_depend>costmap_2d</build_depend>
<!-- new dependencies -->
<!--<build_depend>new build dependency</build_depend>-->
<run_depend>iri_ros_tools</run_depend>
@@ -63,9 +62,8 @@
<run_depend>nav_msgs</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>
<run_depend>iri_adc_msgs</run_depend>
+ <run_depend>costmap_2d</run_depend>
<!-- new dependencies -->
<!--<run_depend>new run dependency</run_depend>-->
diff --git a/setup.py b/setup.py
index ff212ab..179fd22 100644
--- a/setup.py
+++ b/setup.py
@@ -2,7 +2,7 @@
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
- packages=['iri_adc_nav_module'],
+ packages=['iri_nav_module'],
package_dir={'': 'src'}
)
setup(**d)
diff --git a/src/adc_nav_bt_client_alg_node.cpp b/src/adc_nav_bt_client_alg_node.cpp
deleted file mode 100644
index d1e80a4..0000000
--- a/src/adc_nav_bt_client_alg_node.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#include "adc_nav_bt_client_alg_node.h"
-
-ADCNavModuleBtClientAlgNode::ADCNavModuleBtClientAlgNode(void) :
- behaviortree_base::IriBaseBTClient<adc_nav_module::ADCNavBtClientConfig>(),
- adc_nav_module_bt()
-{
- // [init publishers]
-
- // [init subscribers]
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
-
- this->set_go_to_pose = false;
- this->adc_nav_module_bt.init(this->factory);
-
- // Init local functions
- BT::PortsList async_go_to_pose_inputs_ports = {BT::OutputPort<double>("x"), BT::OutputPort<double>("y"), BT::OutputPort<double>("yaw"), BT::OutputPort<double>("heading_tol"), BT::OutputPort<double>("x_y_pos_tol"), BT::OutputPort<bool>("new_goal")};
- BT::PortsList set_goal_frame_inputs_ports = {BT::OutputPort<std::string>("frame_id")};
-
- this->factory.registerIriAsyncAction("set_adc_nav_go_to_pose_inputs", std::bind(&ADCNavModuleBtClientAlgNode::set_adc_nav_go_to_pose_inputs, this, std::placeholders::_1), async_go_to_pose_inputs_ports);
- this->factory.registerIriAsyncAction("set_adc_nav_goal_frame_inputs", std::bind(&ADCNavModuleBtClientAlgNode::set_adc_nav_goal_frame_inputs, this, std::placeholders::_1), set_goal_frame_inputs_ports);
-
-}
-
-ADCNavModuleBtClientAlgNode::~ADCNavModuleBtClientAlgNode(void)
-{
- // [free dynamic memory]
-}
-
-/* [subscriber callbacks] */
-
-/* [service callbacks] */
-
-/* [action callbacks] */
-
-/* [action requests] */
-
-
-void ADCNavModuleBtClientAlgNode::node_config_update(adc_nav_module::ADCNavBtClientConfig &config, uint32_t level)
-{
- this->lock();
-
- if(config.START)
- {
- this->core.start_tree();
- config.START=false;
- }
-
- if(config.RESTART)
- {
- this->core.stop_tree();
- config.RESTART=false;
- }
-
- if (config.set_go_to_input)
- {
- this->set_go_to_pose = true;
- config.set_go_to_input = false;
- }
-
- this->config_=config;
- this->unlock();
-}
-
-BT::NodeStatus ADCNavModuleBtClientAlgNode::set_adc_nav_go_to_pose_inputs(BT::TreeNode& self)
-{
- if (this->set_go_to_pose)
- {
- this->set_go_to_pose = false;
- self.setOutput("x", this->config_.mb_x_pos);
- self.setOutput("y", this->config_.mb_y_pos);
- self.setOutput("yaw", this->config_.mb_yaw);
- self.setOutput("heading_tol", this->config_.mb_xy_tol);
- self.setOutput("x_y_pos_tol", this->config_.mb_yaw_tol);
- self.setOutput("new_goal", true);
- return BT::NodeStatus::SUCCESS;
- }
- return BT::NodeStatus::RUNNING;
-}
-BT::NodeStatus ADCNavModuleBtClientAlgNode::set_adc_nav_goal_frame_inputs(BT::TreeNode& self)
-{
- if (this->set_go_to_pose)
- {
- // this->set_go_to_pose = false;
- self.setOutput("frame_id", this->config_.mb_frame_id);
- return BT::NodeStatus::SUCCESS;
- }
- return BT::NodeStatus::RUNNING;
-}
-
-void ADCNavModuleBtClientAlgNode::addNodeDiagnostics(void)
-{
-}
-
-/* main function */
-int main(int argc,char *argv[])
-{
- return behaviortree_base::main<ADCNavModuleBtClientAlgNode>(argc, argv, "adc_nav_module_bt_client_alg_node");
-}
diff --git a/src/adc_nav_bt_module.cpp b/src/adc_nav_bt_module.cpp
deleted file mode 100644
index f07c693..0000000
--- a/src/adc_nav_bt_module.cpp
+++ /dev/null
@@ -1,655 +0,0 @@
-#include <adc_nav_module/adc_nav_bt_module.h>
-#include <adc_nav_module/adc_nav_module.h>
-
-CADCNavModuleBT::CADCNavModuleBT() :
- adc_nav_module("adc_nav_module",ros::this_node::getName())
-{
-
-}
-
-CADCNavModuleBT::CADCNavModuleBT(std::string &module_name) :
- adc_nav_module(module_name,ros::this_node::getName())
-{
-
-}
-
-void CADCNavModuleBT::init(IriBehaviorTreeFactory &factory)
-{
- //Definition of ports
- BT::PortsList sync_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol")};
- BT::PortsList async_go_to_pose_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
- BT::PortsList sync_go_to_orientation_ports = {BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol")};
- BT::PortsList async_go_to_orientation_ports = {BT::InputPort<double>("yaw"), BT::InputPort<double>("heading_tol"), BT::BidirectionalPort<bool>("new_goal")};
- BT::PortsList sync_go_to_position_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol")};
- BT::PortsList async_go_to_position_ports = {BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
- BT::PortsList set_goal_frame_ports = {BT::InputPort<std::string>("frame_id")};
-
- BT::PortsList auto_clear_ports = {BT::InputPort<double>("rate_hz")};
- BT::PortsList set_bool_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<bool>("value")};
- BT::PortsList set_int_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<int>("value")};
- BT::PortsList set_string_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<std::string>("value")};
- BT::PortsList set_double_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<double>("value")};
- BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::Pose>("pose")};
- BT::PortsList goal_distance_ports = {BT::OutputPort<double>("distance")};
-
-
- //Registration of conditions
- factory.registerSimpleCondition("is_adc_nav_finished", std::bind(&CADCNavModuleBT::is_adc_nav_finished, this));
-
- factory.registerSimpleCondition("is_adc_nav_module_running", std::bind(&CADCNavModuleBT::is_adc_nav_module_running, this));
- factory.registerSimpleCondition("is_adc_nav_module_success", std::bind(&CADCNavModuleBT::is_adc_nav_module_success, this));
- factory.registerSimpleCondition("is_adc_nav_module_action_start_fail", std::bind(&CADCNavModuleBT::is_adc_nav_module_action_start_fail, this));
- factory.registerSimpleCondition("is_adc_nav_module_timeout", std::bind(&CADCNavModuleBT::is_adc_nav_module_timeout, this));
- factory.registerSimpleCondition("is_adc_nav_module_fb_watchdog", std::bind(&CADCNavModuleBT::is_adc_nav_module_fb_watchdog, this));
- factory.registerSimpleCondition("is_adc_nav_module_aborted", std::bind(&CADCNavModuleBT::is_adc_nav_module_aborted, this));
- factory.registerSimpleCondition("is_adc_nav_module_preempted", std::bind(&CADCNavModuleBT::is_adc_nav_module_preempted, this));
- factory.registerSimpleCondition("is_adc_nav_module_rejected", std::bind(&CADCNavModuleBT::is_adc_nav_module_rejected, this));
- factory.registerSimpleCondition("is_adc_nav_module_set_param_fail", std::bind(&CADCNavModuleBT::is_adc_nav_module_set_param_fail, this));
- factory.registerSimpleCondition("is_adc_nav_module_param_not_present", std::bind(&CADCNavModuleBT::is_adc_nav_module_param_not_present, this));
- factory.registerSimpleCondition("is_adc_nav_module_no_odom", std::bind(&CADCNavModuleBT::is_adc_nav_module_no_odom, this));
- factory.registerSimpleCondition("is_adc_nav_module_no_transform", std::bind(&CADCNavModuleBT::is_adc_nav_module_no_transform, this));
-
- //Registration of actions
- factory.registerIriAsyncAction("async_is_adc_nav_finished", std::bind(&CADCNavModuleBT::async_is_adc_nav_finished, this));
-
- factory.registerSimpleAction("sync_cancel_adc_nav_action", std::bind(&CADCNavModuleBT::sync_cancel_adc_nav_action, this));
- factory.registerSimpleAction("async_cancel_adc_nav_action", std::bind(&CADCNavModuleBT::async_cancel_adc_nav_action, this));
-
- factory.registerSimpleAction("sync_adc_nav_go_to_pose", std::bind(&CADCNavModuleBT::sync_adc_nav_go_to_pose, this, std::placeholders::_1), sync_go_to_pose_ports);
- factory.registerIriAsyncAction("async_adc_nav_go_to_pose", std::bind(&CADCNavModuleBT::async_adc_nav_go_to_pose, this, std::placeholders::_1), async_go_to_pose_ports);
- factory.registerSimpleAction("sync_adc_nav_go_to_orientation", std::bind(&CADCNavModuleBT::sync_adc_nav_go_to_orientation, this, std::placeholders::_1), sync_go_to_orientation_ports);
- factory.registerIriAsyncAction("async_adc_nav_go_to_orientation", std::bind(&CADCNavModuleBT::async_adc_nav_go_to_orientation, this, std::placeholders::_1), async_go_to_orientation_ports);
- factory.registerSimpleAction("sync_adc_nav_go_to_position", std::bind(&CADCNavModuleBT::sync_adc_nav_go_to_position, this, std::placeholders::_1), sync_go_to_position_ports);
- factory.registerIriAsyncAction("async_adc_nav_go_to_position", std::bind(&CADCNavModuleBT::async_adc_nav_go_to_position, this, std::placeholders::_1), async_go_to_position_ports);
- factory.registerSimpleAction("set_adc_nav_goal_frame", std::bind(&CADCNavModuleBT::set_adc_nav_goal_frame, this, std::placeholders::_1), set_goal_frame_ports);
-
- factory.registerSimpleAction("adc_nav_costmaps_clear", std::bind(&CADCNavModuleBT::adc_nav_costmaps_clear, this));
- factory.registerSimpleAction("adc_nav_costmaps_enable_auto_clear", std::bind(&CADCNavModuleBT::adc_nav_costmaps_enable_auto_clear, this, std::placeholders::_1), auto_clear_ports);
- factory.registerSimpleAction("adc_nav_costmaps_disable_auto_clear", std::bind(&CADCNavModuleBT::adc_nav_costmaps_disable_auto_clear, this));
- factory.registerSimpleAction("adc_nav_set_bool_parameter", std::bind(&CADCNavModuleBT::adc_nav_set_bool_parameter, this, std::placeholders::_1), set_bool_parameter_ports);
- factory.registerSimpleAction("adc_nav_set_int_parameter", std::bind(&CADCNavModuleBT::adc_nav_set_int_parameter, this, std::placeholders::_1), set_int_parameter_ports);
- factory.registerSimpleAction("adc_nav_set_string_parameter", std::bind(&CADCNavModuleBT::adc_nav_set_string_parameter, this, std::placeholders::_1), set_string_parameter_ports);
- factory.registerSimpleAction("adc_nav_set_double_parameter", std::bind(&CADCNavModuleBT::adc_nav_set_double_parameter, this, std::placeholders::_1), set_double_parameter_ports);
- factory.registerSimpleAction("get_adc_nav_current_pose", std::bind(&CADCNavModuleBT::get_adc_nav_current_pose, this, std::placeholders::_1), current_pose_ports);
- factory.registerSimpleAction("get_adc_nav_goal_distance", std::bind(&CADCNavModuleBT::get_adc_nav_goal_distance, this, std::placeholders::_1), goal_distance_ports);
-}
-
-CADCNavModuleBT::~CADCNavModuleBT(void)
-{
- // [free dynamic memory]
-}
-
-BT::NodeStatus CADCNavModuleBT::sync_adc_nav_go_to_pose(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::sync_adc_nav_go_to_pose-> sync_adc_nav_go_to_pose");
- BT::Optional<double> x = self.getInput<double>("x");
- BT::Optional<double> y = self.getInput<double>("y");
- BT::Optional<double> yaw = self.getInput<double>("yaw");
- BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
- BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
-
- double x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal;
- if (!x || !y || !yaw || (!heading_tol && x_y_pos_tol))
- {
- ROS_ERROR("CADCNavModuleBT::sync_adc_nav_go_to_pose-> Incorrect or missing input. It needs the following input ports: x(double), y(double), yaw(double), [Optional] heading_tol(double) and [Optional] x_y_pos_tol(double)");
- ROS_ERROR("CADCNavModuleBT::sync_adc_nav_go_to_pose-> The correct input options are: x + y + yaw, x + y + yaw + heading_tol or x + y + yaw + heading_tol + x_y_pos_tol");
- return BT::NodeStatus::FAILURE;
- }
-
- x_goal = x.value();
- y_goal = y.value();
- yaw_goal = yaw.value();
- if (!x_y_pos_tol)
- {
- if (!heading_tol)
- {
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- x_y_pos_tol_goal = x_y_pos_tol.value();
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::async_adc_nav_go_to_pose(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::async_adc_nav_go_to_pose-> async_adc_nav_go_to_pose");
- BT::Optional<double> x = self.getInput<double>("x");
- BT::Optional<double> y = self.getInput<double>("y");
- BT::Optional<double> yaw = self.getInput<double>("yaw");
- BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
- BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
- BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
-
- bool new_goal;
- if (!new_goal_in)
- new_goal = false;
- else
- new_goal = new_goal_in.value();
-
- if (new_goal)
- {
- double x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal;
- if (!x || !y || !yaw || (!heading_tol && x_y_pos_tol))
- {
- ROS_ERROR("CADCNavModuleBT::async_adc_nav_go_to_pose-> Incorrect or missing input. It needs the following input ports: x(double), y(double), yaw(double), [Optional] heading_tol(double) and [Optional] x_y_pos_tol(double)");
- ROS_ERROR("CADCNavModuleBT::async_adc_nav_go_to_pose-> The correct input options are: x + y + yaw, x + y + yaw + heading_tol or x + y + yaw + heading_tol + x_y_pos_tol");
- return BT::NodeStatus::FAILURE;
- }
-
- x_goal = x.value();
- y_goal = y.value();
- yaw_goal = yaw.value();
- if (!x_y_pos_tol)
- {
- if (!heading_tol)
- {
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- x_y_pos_tol_goal = x_y_pos_tol.value();
- if (!this->adc_nav_module.go_to_pose(x_goal, y_goal, yaw_goal, heading_tol_goal, x_y_pos_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- }
- if (this->adc_nav_module.is_finished())
- {
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
- if (adc_nav_module_status == ADC_NAV_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::FAILURE;
- }
- if (new_goal)
- self.setOutput("new_goal", false);
- return BT::NodeStatus::RUNNING;
-}
-
-BT::NodeStatus CADCNavModuleBT::sync_adc_nav_go_to_orientation(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::sync_adc_nav_go_to_orientation-> sync_adc_nav_go_to_orientation");
- BT::Optional<double> yaw = self.getInput<double>("yaw");
- BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
-
- double yaw_goal, heading_tol_goal;
- if (!yaw)
- {
- ROS_ERROR("CADCNavModuleBT::sync_adc_nav_go_to_orientation-> Incorrect or missing input. It needs the following input ports: yaw(double) and [Optional] heading_tol(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- yaw_goal = yaw.value();
- if (!heading_tol)
- {
- if (!this->adc_nav_module.go_to_orientation (yaw_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- if (!this->adc_nav_module.go_to_orientation (yaw_goal, heading_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::async_adc_nav_go_to_orientation(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::async_adc_nav_go_to_orientation-> async_adc_nav_go_to_orientation");
- BT::Optional<double> yaw = self.getInput<double>("yaw");
- BT::Optional<double> heading_tol = self.getInput<double>("heading_tol");
- BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
-
- bool new_goal;
- if (!new_goal_in)
- new_goal = false;
- else
- new_goal = new_goal_in.value();
-
- if (new_goal)
- {
- double yaw_goal, heading_tol_goal;
- if (!yaw)
- {
- ROS_ERROR("CADCNavModuleBT::async_adc_nav_go_to_orientation-> Incorrect or missing input. It needs the following input ports: yaw(double) and [Optional] heading_tol(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- yaw_goal = yaw.value();
- if (!heading_tol)
- {
- if (!this->adc_nav_module.go_to_orientation(yaw_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- heading_tol_goal = heading_tol.value();
- if (!this->adc_nav_module.go_to_orientation(yaw_goal, heading_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- }
- if (this->adc_nav_module.is_finished())
- {
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
- if (adc_nav_module_status == ADC_NAV_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::FAILURE;
- }
- if (new_goal)
- self.setOutput("new_goal", false);
- return BT::NodeStatus::RUNNING;
-}
-
-BT::NodeStatus CADCNavModuleBT::sync_adc_nav_go_to_position(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::sync_adc_nav_go_to_position-> sync_adc_nav_go_to_position");
- BT::Optional<double> x = self.getInput<double>("x");
- BT::Optional<double> y = self.getInput<double>("y");
- BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
-
- double x_goal, y_goal, x_y_pos_tol_goal;
- if (!x || !y)
- {
- ROS_ERROR("CADCNavModuleBT::sync_adc_nav_go_to_position-> Incorrect or missing input. It needs the following input ports: x(double), y(double) and [Optional] x_y_pos_tol(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- x_goal = x.value();
- y_goal = y.value();
- if (!x_y_pos_tol)
- {
- if (!this->adc_nav_module.go_to_position(x_goal, y_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- x_y_pos_tol_goal = x_y_pos_tol.value();
- if (!this->adc_nav_module.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::async_adc_nav_go_to_position(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::async_adc_nav_go_to_position-> async_adc_nav_go_to_position");
- BT::Optional<double> x = self.getInput<double>("x");
- BT::Optional<double> y = self.getInput<double>("y");
- BT::Optional<double> x_y_pos_tol = self.getInput<double>("x_y_pos_tol");
- BT::Optional<bool> new_goal_in = self.getInput<bool>("new_goal");
-
- bool new_goal;
- if (!new_goal_in)
- new_goal = false;
- else
- new_goal = new_goal_in.value();
-
- if (new_goal)
- {
- double x_goal, y_goal, x_y_pos_tol_goal;
- if (!x || !y)
- {
- ROS_ERROR("CADCNavModuleBT::async_adc_nav_go_to_position-> Incorrect or missing input. It needs the following input ports: x(double), y(double) and [Optional] x_y_pos_tol(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- x_goal = x.value();
- y_goal = y.value();
- if (!x_y_pos_tol)
- {
- if (!this->adc_nav_module.go_to_position(x_goal, y_goal))
- return BT::NodeStatus::FAILURE;
- }
- else
- {
- x_y_pos_tol_goal = x_y_pos_tol.value();
- if (!this->adc_nav_module.go_to_position(x_goal, y_goal, x_y_pos_tol_goal))
- return BT::NodeStatus::FAILURE;
- }
- }
- if (this->adc_nav_module.is_finished())
- {
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
- if (adc_nav_module_status == ADC_NAV_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::FAILURE;
- }
- if (new_goal)
- self.setOutput("new_goal", false);
- return BT::NodeStatus::RUNNING;
-}
-
-BT::NodeStatus CADCNavModuleBT::set_adc_nav_goal_frame(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::set_adc_nav_goal_frame-> set_adc_nav_goal_frame");
- BT::Optional<std::string> frame_id = self.getInput<std::string>("frame_id");
-
- std::string frame_id_goal;
- if (!frame_id)
- {
- ROS_ERROR("CADCNavModuleBT::set_adc_nav_goal_frame-> Incorrect or missing input. It needs the following input ports: frame_id(std::string)");
- return BT::NodeStatus::FAILURE;
- }
-
- frame_id_goal = frame_id.value();
-
- this->adc_nav_module.set_goal_frame(frame_id_goal);
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_costmaps_clear(void)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_costmaps_clear-> adc_nav_costmaps_clear");
- if (this->adc_nav_module.costmaps_clear())
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_costmaps_enable_auto_clear(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_costmaps_enable_auto_clear-> adc_nav_costmaps_enable_auto_clear");
- BT::Optional<double> rate_hz = self.getInput<double>("rate_hz");
-
- double rate_hz_goal;
- if (!rate_hz)
- {
- ROS_ERROR("CADCNavModuleBT::adc_nav_costmaps_enable_auto_clear-> Incorrect or missing input. It needs the following input ports: rate_hz(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- rate_hz_goal = rate_hz.value();
-
- this->adc_nav_module.costmaps_enable_auto_clear(rate_hz_goal);
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_costmaps_disable_auto_clear(void)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_costmaps_disable_auto_clear-> adc_nav_costmaps_disable_auto_clear");
- this->adc_nav_module.costmaps_disable_auto_clear();
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_set_bool_parameter(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_set_bool_parameter-> adc_nav_set_bool_parameter");
- BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
- BT::Optional<std::string> name = self.getInput<std::string>("name");
- BT::Optional<bool> value = self.getInput<bool>("value");
-
- std::string name_space_goal, name_goal;
- bool value_goal;
- if (!name_space || !name || !value)
- {
- ROS_ERROR("CADCNavModuleBT::adc_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(bool)");
- return BT::NodeStatus::FAILURE;
- }
-
- name_space_goal = name_space.value();
- name_goal = name.value();
- value_goal = value.value();
-
- if (this->adc_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_set_int_parameter(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_set_int_parameter-> adc_nav_set_int_parameter");
- BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
- BT::Optional<std::string> name = self.getInput<std::string>("name");
- BT::Optional<int> value = self.getInput<int>("value");
-
- std::string name_space_goal, name_goal;
- int value_goal;
- if (!name_space || !name || !value)
- {
- ROS_ERROR("CADCNavModuleBT::adc_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(int)");
- return BT::NodeStatus::FAILURE;
- }
-
- name_space_goal = name_space.value();
- name_goal = name.value();
- value_goal = value.value();
-
- if (this->adc_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_set_string_parameter(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_set_string_parameter-> adc_nav_set_string_parameter");
- BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
- BT::Optional<std::string> name = self.getInput<std::string>("name");
- BT::Optional<std::string> value = self.getInput<std::string>("value");
-
- std::string name_space_goal, name_goal;
- std::string value_goal;
- if (!name_space || !name || !value)
- {
- ROS_ERROR("CADCNavModuleBT::adc_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(std::string)");
- return BT::NodeStatus::FAILURE;
- }
-
- name_space_goal = name_space.value();
- name_goal = name.value();
- value_goal = value.value();
-
- if (this->adc_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::adc_nav_set_double_parameter(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::adc_nav_set_double_parameter-> adc_nav_set_double_parameter");
- BT::Optional<std::string> name_space = self.getInput<std::string>("name_space");
- BT::Optional<std::string> name = self.getInput<std::string>("name");
- BT::Optional<double> value = self.getInput<double>("value");
-
- std::string name_space_goal, name_goal;
- double value_goal;
- if (!name_space || !name || !value)
- {
- ROS_ERROR("CADCNavModuleBT::adc_nav_set_bool_parameter-> Incorrect or missing input. It needs the following input ports: name_space(std::string), name(std::string) and value(double)");
- return BT::NodeStatus::FAILURE;
- }
-
- name_space_goal = name_space.value();
- name_goal = name.value();
- value_goal = value.value();
-
- if (this->adc_nav_module.set_parameter(name_space_goal, name_goal, value_goal))
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::get_adc_nav_current_pose(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::get_adc_nav_current_pose-> get_adc_nav_current_pose");
-
- geometry_msgs::Pose pose_out = this->adc_nav_module.getCurrentPose();
-
- self.setOutput("pose", pose_out);
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::get_adc_nav_goal_distance(BT::TreeNode& self)
-{
- ROS_DEBUG("CADCNavModuleBT::get_adc_nav_goal_distance-> get_adc_nav_goal_distance");
-
- double dist = this->adc_nav_module.getGoalDistance();
- self.setOutput("distance", dist);
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::sync_cancel_adc_nav_action(void)
-{
- ROS_DEBUG("CADCNavModuleBT::sync_cancel_adc_nav_action-> sync_cancel_adc_nav_action");
- this->adc_nav_module.stop();
- return BT::NodeStatus::SUCCESS;
-}
-
-BT::NodeStatus CADCNavModuleBT::async_cancel_adc_nav_action(void)
-{
- ROS_DEBUG("CADCNavModuleBT::async_cancel_adc_nav_action-> async_cancel_adc_nav_action");
- this->adc_nav_module.stop();
- if (this->adc_nav_module.is_finished())
- {
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
- if (adc_nav_module_status == ADC_NAV_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- else
- return BT::NodeStatus::FAILURE;
- }
- return BT::NodeStatus::RUNNING;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_finished(void)
-{
- if (this->adc_nav_module.is_finished())
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::async_is_adc_nav_finished(void)
-{
- if (this->adc_nav_module.is_finished())
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::RUNNING;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_running(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_RUNNING)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_success(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_SUCCESS)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_action_start_fail(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_ACTION_START_FAIL)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_timeout(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_TIMEOUT)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_fb_watchdog(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_FB_WATCHDOG)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_aborted(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_ABORTED)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_preempted(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_PREEMPTED)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_rejected(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_REJECTED)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_set_param_fail(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_SET_PARAM_FAIL)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_param_not_present(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_PARAM_NOT_PRESENT)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_no_odom(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_NO_ODOM)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
-
-BT::NodeStatus CADCNavModuleBT::is_adc_nav_module_no_transform(void)
-{
- adc_nav_module_status_t adc_nav_module_status = this->adc_nav_module.get_status();
-
- if (adc_nav_module_status == ADC_NAV_MODULE_NO_TRANSFORM)
- return BT::NodeStatus::SUCCESS;
- return BT::NodeStatus::FAILURE;
-}
diff --git a/src/adc_nav_client_alg.cpp b/src/adc_nav_client_alg.cpp
deleted file mode 100644
index 64b5eb0..0000000
--- a/src/adc_nav_client_alg.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "adc_nav_client_alg.h"
-
-ADCNavClientAlgorithm::ADCNavClientAlgorithm(void)
-{
- pthread_mutex_init(&this->access_,NULL);
-}
-
-ADCNavClientAlgorithm::~ADCNavClientAlgorithm(void)
-{
- pthread_mutex_destroy(&this->access_);
-}
-
-void ADCNavClientAlgorithm::config_update(Config& new_cfg, uint32_t level)
-{
- this->lock();
-
- // save the current configuration
- this->config_=new_cfg;
-
- this->unlock();
-}
-
-// ADCNavClientAlgorithm Public API
diff --git a/src/adc_nav_client_alg_node.cpp b/src/adc_nav_client_alg_node.cpp
deleted file mode 100644
index eab6a41..0000000
--- a/src/adc_nav_client_alg_node.cpp
+++ /dev/null
@@ -1,139 +0,0 @@
-#include "adc_nav_client_alg_node.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>(),
- nav("nav_module",ros::this_node::getName())
-{
- //init class attributes if necessary
- //this->loop_rate_ = 2;//in [Hz]
-
- // [init publishers]
-
- // [init subscribers]
-
- // [init services]
-
- // [init clients]
-
- // [init nav servers]
-
- // [init nav clients]
-}
-
-ADCNavClientAlgNode::~ADCNavClientAlgNode(void)
-{
- // [free dynamic memory]
-}
-
-void ADCNavClientAlgNode::mainNodeThread(void)
-{
- // [fill msg structures]
-
- // [fill srv structure and make request to the server]
-
- // [fill nav structure and make request to the nav server]
-
- // [publish messages]
-}
-
-/* [subscriber callbacks] */
-
-/* [service callbacks] */
-
-/* [nav callbacks] */
-
-void ADCNavClientAlgNode::node_config_update(Config &config, uint32_t level)
-{
- this->alg_.lock();
- if(config.start_pose_nav)
- {
- 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_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_id(config.mb_frame_id);
- this->nav.go_to_orientation(config.mb_yaw,config.mb_yaw_tol);
- config.start_orientation_nav=false;
- }
- else if(config.stop_nav)
- {
- this->nav.stop();
- config.stop_nav=false;
- }
- if(config.cm_clear_costmaps)
- {
- this->nav.costmaps_clear();
- config.cm_clear_costmaps=false;
- }
- else if(config.cm_enable_auto_clear)
- {
- this->nav.costmaps_enable_auto_clear(config.cm_auto_clear_rate_hz);
- config.cm_enable_auto_clear=false;
- }
- else if(config.cm_disable_auto_clear)
- {
- this->nav.costmaps_disable_auto_clear();
- config.cm_disable_auto_clear=false;
- }
- 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();
-}
-
-void ADCNavClientAlgNode::addNodeDiagnostics(void)
-{
-}
-
-/* main function */
-int main(int argc,char *argv[])
-{
- return algorithm_base::main<ADCNavClientAlgNode>(argc, argv, "adc_nav_client_alg_node");
-}
diff --git a/src/adc_nav_module.cpp b/src/adc_nav_module.cpp
deleted file mode 100644
index 2450d7a..0000000
--- a/src/adc_nav_module.cpp
+++ /dev/null
@@ -1,239 +0,0 @@
-#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("local_planner",this->module_nh.getNamespace()),
- COpendriveGPModule("global_planner",this->module_nh.getNamespace())
-{
- this->start_operation();
-
- //Variables
- 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;
-}
-
-/* State machine */
-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;
- 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->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->start_action(this->goal_x,this->goal_y,this->goal_yaw))
- {
- case ACT_SRV_SUCCESS: this->state=ADC_NAV_MODULE_WAIT;
- ROS_INFO("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->get_action_status())
- {
- 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->new_goal)
- {
- this->new_goal=false;
- this->state=ADC_NAV_MODULE_TRANSFORM;
- }
- if(this->cancel_pending)
- {
- this->cancel_pending=false;
- this->cancel_action();
- }
- break;
- }
-}
-
-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)
-{
- double xy_tol,yaw_tol;
-
- this->lock();
- if(heading_tol!=DEFAULT_HEADING_TOL)
- {
- if(this->get_goal_tolerances(xy_tol,yaw_tol))
- {
- yaw_tol=heading_tol;
- if(this->set_goal_tolerances(xy_tol,yaw_tol))
- {
- this->unlock();
- return false;
- }
- }
- else
- {
- this->unlock();
- return false;
- }
- }
- /* store the new information goal */
- this->goal_x=0.0;
- this->goal_y=0.0;
- this->goal_yaw=yaw;
- this->new_goal=true;
- this->unlock();
-
- return true;
-}
-
-bool CADCNavModule::go_to_position(double x,double y,double x_y_pos_tol)
-{
- double xy_tol,yaw_tol;
-
- this->lock();
- if(x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
- {
- if(this->get_goal_tolerances(xy_tol,yaw_tol))
- {
- xy_tol=x_y_pos_tol;
- if(this->set_goal_tolerances(xy_tol,yaw_tol))
- {
- this->unlock();
- return false;
- }
- }
- else
- {
- this->unlock();
- return false;
- }
- }
- /* store the new information goal */
- this->goal_x=x;
- this->goal_y=y;
- this->goal_yaw=0.0;
- this->new_goal=true;
- this->unlock();
-
- return true;
-}
-
-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(heading_tol!=DEFAULT_HEADING_TOL || x_y_pos_tol!=DEFAULT_X_Y_POS_TOL)
- {
- if(this->get_goal_tolerances(xy_tol,yaw_tol))
- {
- 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->unlock();
- return false;
- }
- }
- else
- {
- this->unlock();
- return false;
- }
- }
- /* store the new information goal */
- this->goal_x=x;
- this->goal_y=y;
- this->goal_yaw=yaw;
- this->new_goal=true;
- this->unlock();
-
- return true;
-}
-
-void CADCNavModule::stop(void)
-{
- if(this->state!=ADC_NAV_MODULE_IDLE)
- this->cancel_pending=true;
-}
-
-bool CADCNavModule::is_finished(void)
-{
- if(this->state==ADC_NAV_MODULE_IDLE && this->new_goal==false)
- return true;
- else
- return false;
-}
-
-adc_nav_module_status_t CADCNavModule::get_status(void)
-{
- return this->status;
-}
-
-CADCNavModule::~CADCNavModule()
-{
-}
diff --git a/src/iri_adc_nav_module/dyn_params.pyc b/src/iri_adc_nav_module/dyn_params.pyc
deleted file mode 100644
index 3c8e0755e31367a17eb059cb19931ea0b1c836f3..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 1678
zcmcgsUvC^W5TD(<OVU7S)3i+styn-vr%KI9@QMghr1C^T*YJ`T%eVGz-0rSV_FO_r
z%1ins;6w1v$Ke|w@d4mB-n(26#4Enr@p%4uJoB@Af9`B=egBg#X)$ulU*U_NgBbiX
zs)>$;%c;(&$>}7grbj0|YWj52r_A)|4bjPfOrM?)2k>)$VGJ{HEadU9C+q_dZH#KX
znb%fD8_ry5)l3K7M83bS)g?RME|t6M%Fd;fT(=hA5$1S=-7bk<fM#@+(d(Qp^TiSe
z*%wjoEm7Wy`jRFL;e@m7U*(i>@)SWo30@EAazHQO4AFr0xld+;%qE#Fx*AZnQr>_P
zrGRpqc%xJHmY(lG2_GomC9^~3Ju)}xY7_qbrOB;plU*{m-!kE?WsZAYN*i>!vGlrg
zz3W}#8{Mwg%6r!#_sQH(As?h&S0Uf;LT<_y&xaqNn9mS~NPTO3V>w)~v``&CMf#Y#
z7V>t-pIJ50(emEfQcclUtDJEVQ~VTUY^OHZwzNgk6@FUqsV!zcI_Y*%d0oe}Q=?~m
zZ#`V8;aIC#uxC+JM`>Ni+WC1bI>hi@@$$%8oog;njjt3t<Pqb?(0rqRaLv3aL{v;H
zpIh4&VqG*gMqSzH1hLhRcYgi)_g8NY-Dg<wZHdX#6v)r{KLfHa&b8xW>O;Xnw-HR^
z<VNrgNLgDQROutj@d^pLCQ`Q56Wk0vXWwb!hcNoi2Iq|{bzNT+$stv35hwCT{|>Z@
zpn_(rPG3c-%aJ5dEFFtk3S9m2vy9&kD_CIsahN*a=$E*gwQh&f<s_2JTLnL#jRg#2
zxqhixkOT7QF$cxKJkiAa8f+izahO}+4DW}Z%i(w|1y1%)(QP{vT?$j#HhHvwP1>)u
z=iPouEM3C2gz=PbVQTyWcWTjjLFVFYV=)-w>qwk}8=V5lBfL758@b?~)OsSbVyl6b
zIEtYhwff9eTCkW@+LQ{S1>BQ`c>F_bE|%}q!R#X8PYSj2TRdW^LJuXU{ARwF?d7|m
zkFrPkC-?~mnm-##`S<_?VRhq56>RC-sjKF}9;AoQvJDEwvS_Ubo;1I7ZF)Z?x!|K>
ols_J<@=7I7{pva=s9IGgasOwDq~l+7Pso?j&?q?B&K~A}1H|^X$^ZZW
diff --git a/src/iri_adc_nav_module/__init__.py b/src/iri_nav_module/__init__.py
similarity index 100%
rename from src/iri_adc_nav_module/__init__.py
rename to src/iri_nav_module/__init__.py
diff --git a/src/iri_adc_nav_module/dyn_params.py b/src/iri_nav_module/dyn_params.py
similarity index 87%
rename from src/iri_adc_nav_module/dyn_params.py
rename to src/iri_nav_module/dyn_params.py
index c396cdb..4de336e 100644
--- a/src/iri_adc_nav_module/dyn_params.py
+++ b/src/iri_nav_module/dyn_params.py
@@ -27,3 +27,11 @@ def add_nav_module_params(gen,name):
return new_group
+def add_opendrive_gp_module_params(gen,name):
+
+ new_group = gen.add_group(name)
+
+ add_module_service_params(new_group,name+"_get_map")
+ add_module_service_params(new_group,name+"_get_nodes")
+
+ return new_group
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
deleted file mode 100644
index 63ecdbd..0000000
--- a/src/xml/bt_test.xml
+++ /dev/null
@@ -1,184 +0,0 @@
-<?xml version="1.0"?>
-<root main_tree_to_execute="BehaviorTree">
- <!-- ////////// -->
- <BehaviorTree ID="BehaviorTree">
- <SequenceStar>
- <Action ID="async_is_start_tree"/>
- <SubTree ID="move_to_pose"/>
- <SubTree ID="move_to_position"/>
- <SubTree ID="move_to_orientation"/>
- </SequenceStar>
- </BehaviorTree>
- <!-- ////////// -->
- <BehaviorTree ID="move_to_pose">
- <SequenceStar>
- <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
- <Action ID="async_tiago_nav_go_to_pose" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
- </SequenceStar>
- </BehaviorTree>
- <!-- ////////// -->
- <BehaviorTree ID="move_to_position">
- <SequenceStar>
- <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
- <Action ID="async_tiago_nav_go_to_position" x="{x}" y="{y}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
- </SequenceStar>
- </BehaviorTree>
- <!-- ////////// -->
- <BehaviorTree ID="move_to_orientation">
- <SequenceStar>
- <Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
- <Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
- <Action ID="async_tiago_nav_go_to_orientation" yaw="{yaw}" heading_tol="{heading_tol}" new_goal="{new_goal}"/>
- </SequenceStar>
- </BehaviorTree>
- <!-- ////////// -->
- <TreeNodesModel>
- <!-- basic_nodes -->
- <Action ID="NOP"/>
- <Action ID="is_not_start_tree"/>
- <Condition ID="async_is_start_tree"/>
- <Action ID="is_not_stop_tree"/>
- <Condition ID="async_is_stop_tree"/>
- <Action ID="set_start_tree"/>
- <Action ID="set_stop_tree"/>
- <Action ID="reset_start_tree"/>
- <Action ID="reset_stop_tree"/>
- <!-- tiago_nav_module -->
- <Action ID="sync_cancel_tiago_nav_action"/>
- <Action ID="async_cancel_tiago_nav_action"/>
- <Action ID="async_is_tiago_nav_finished"/>
- <Action ID="sync_tiago_nav_go_to_pose">
- <input_port name="x"> desired X positon with respect to the goal frame</input_port>
- <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
- <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
- <input_port name="heading_tol"> heading tolerance for the goal</input_port>
- <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
- </Action>
- <Action ID="async_tiago_nav_go_to_pose">
- <input_port name="x"> desired X positon with respect to the goal frame</input_port>
- <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
- <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
- <input_port name="heading_tol"> heading tolerance for the goal</input_port>
- <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
- <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
- </Action>
- <Action ID="sync_tiago_nav_go_to_orientation">
- <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
- <input_port name="heading_tol"> heading tolerance for the goal</input_port>
- </Action>
- <Action ID="async_tiago_nav_go_to_orientation">
- <input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
- <input_port name="heading_tol"> heading tolerance for the goal</input_port>
- <input_port name="new_goal"> If it's a new_goal</input_port>
- </Action>
- <Action ID="sync_tiago_nav_go_to_position">
- <input_port name="x"> desired X positon with respect to the goal frame</input_port>
- <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
- <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
- </Action>
- <Action ID="async_tiago_nav_go_to_position">
- <input_port name="x"> desired X positon with respect to the goal frame</input_port>
- <input_port name="y"> desired Y positon with respect to the goal frame</input_port>
- <input_port name="x_y_pos_tol"> position tolerance for the goal</input_port>
- <input_port name="new_goal"> If it's a new_goal</input_port>
- </Action>
- <Action ID="sync_tiago_nav_go_to_poi">
- <input_port name="poi"> name of the desired point of interest to move to</input_port>
- </Action>
- <Action ID="async_tiago_nav_go_to_poi">
- <input_port name="poi"> name of the desired point of interest to move to</input_port>
- </Action>
- <Action ID="sync_tiago_nav_go_to_waypoints">
- <input_port name="group"> Name of the desired group of points of interest</input_port>
- <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
- <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
- <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
- </Action>
- <Action ID="async_tiago_nav_go_to_waypoints">
- <input_port name="group"> Name of the desired group of points of interest</input_port>
- <input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
- <input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
- <input_port name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
- <input_port name="new_goal"> If it's a new_goal</input_port>
- </Action>
- <Action ID="get_tiago_nav_current_waypoint">
- <output_port name="waypoint"> The index of the current waypoint being executed</output_port>
- </Action>
- <Action ID="set_tiago_nav_goal_frame">
- <input_port name="frame_id"> The name of the new reference frame for the future goals</input_port>
- </Action>
- <Action ID="tiago_nav_map_change">
- <input_port name="map_name"> name of the desired map</input_port>
- </Action>
- <Action ID="tiago_nav_costmaps_clear"/>
- <Action ID="tiago_nav_costmaps_enable_auto_clear">
- <input_port name="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</input_port>
- </Action>
- <Action ID="tiago_nav_costmaps_disable_auto_clear"/>
- <Action ID="tiago_nav_set_bool_parameter">
- <input_port name="name_space"> String with the parameter name space</input_port>
- <input_port name="name"> String with the name of the parameter to change</input_port>
- <input_port name="value"> the new value for the parameter</input_port>
- </Action>
- <Action ID="tiago_nav_set_int_parameter">
- <input_port name="name_space"> String with the parameter name space</input_port>
- <input_port name="name"> String with the name of the parameter to change</input_port>
- <input_port name="value"> the new value for the parameter</input_port>
- </Action>
- <Action ID="tiago_nav_set_string_parameter">
- <input_port name="name_space"> String with the parameter name space</input_port>
- <input_port name="name"> String with the name of the parameter to change</input_port>
- <input_port name="value"> the new value for the parameter</input_port>
- </Action>
- <Action ID="tiago_nav_set_double_parameter">
- <input_port name="name_space"> String with the parameter name space</input_port>
- <input_port name="name"> String with the name of the parameter to change</input_port>
- <input_port name="value"> the new value for the parameter</input_port>
- </Action>
- <Action ID="get_tiago_nav_current_pose">
- <output_port name="pose"> current pose</output_port>
- </Action>
- <Action ID="get_tiago_goal_distance">
- <output_port name="distance"> current goal distance</output_port>
- </Action>
- <Condition ID="is_tiago_nav_finished"/>
- <Condition ID="is_tiago_nav_module_running"/>
- <Condition ID="is_tiago_nav_module_success"/>
- <Condition ID="is_tiago_nav_module_action_start_fail"/>
- <Condition ID="is_tiago_nav_module_timeout"/>
- <Condition ID="is_tiago_nav_module_fb_watchdog"/>
- <Condition ID="is_tiago_nav_module_aborted"/>
- <Condition ID="is_tiago_nav_module_preempted"/>
- <Condition ID="is_tiago_nav_module_rejected"/>
- <Condition ID="is_tiago_nav_module_set_param_fail"/>
- <Condition ID="is_tiago_nav_module_param_not_present"/>
- <Condition ID="is_tiago_nav_module_no_odom"/>
- <Condition ID="is_tiago_nav_module_no_transform"/>
- <Condition ID="is_tiago_nav_module_unknown_map"/>
- <Condition ID="is_tiago_nav_module_invalid_mode"/>
- <!-- Client -->
- <Action ID="set_tiago_nav_go_to_pose_inputs">
- <output_port name="x"> desired X positon with respect to the goal frame</output_port>
- <output_port name="y"> desired Y positon with respect to the goal frame</output_port>
- <output_port name="yaw"> desired orientation with respect to the goal frame</output_port>
- <output_port name="heading_tol"> heading tolerance for the goal</output_port>
- <output_port name="x_y_pos_tol"> position tolerance for the goal</output_port>
- <output_port name="new_goal"> If it's a new_goal</output_port>
- </Action>
- <Action ID="set_tiago_nav_goal_frame_inputs">
- <output_port name="frame_id"> The name of the new reference frame for the future goals</output_port>
- </Action>
- <!-- Tree -->
- <SubTree ID="move_to_pose"/>
- <SubTree ID="move_to_position"/>
- <SubTree ID="move_to_posorientation"/>
- </TreeNodesModel>
- <!-- ////////// -->
-</root>
-
-
--
GitLab