Skip to content
Snippets Groups Projects
Commit dd069dbc authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Initial commit

parents
No related branches found
No related tags found
No related merge requests found
Showing
with 2347 additions and 0 deletions
cmake_minimum_required(VERSION 2.8.3)
project(adc_nav_module)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(iriutils REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## 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
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
set(module_name adc_nav_module)
set(module_bt_name adc_nav_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
include_directories(${iriutils_INCLUDE_DIR})
## Declare a C++ libraryi
# Module
add_library(${module_name}
src/adc_nav_module.cpp
)
target_link_libraries(${module_name} ${catkin_LIBRARIES})
target_link_libraries(${module_name} ${iriutils_LIBRARY})
##Link to other modules
##target_link_libraries(new_module <module path>/lib<module_name>.so)
## 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})
## 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 ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_humanoid_modules.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
This diff is collapsed.
#! /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"))
#! /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 *
gen = ParameterGenerator()
move_base = gen.add_group("Move base action")
costmaps = gen.add_group("Costmap")
map = gen.add_group("Map")
poi = gen.add_group("Got to point of interest action")
waypoint = gen.add_group("Got to waypoints action")
# 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")
map.add("map_name", str_t, 0, "Name of the map to use", "iri_map")
map.add("map_change", bool_t, 0, "Update the desired map", False)
costmaps.add("cm_enable_auto_clear",bool_t, 0, "Enable costmaps autoclear", False)
costmaps.add("cm_disable_auto_clear",bool_t, 0, "Disable costmaps autoclear", False)
costmaps.add("cm_auto_clear_rate_hz",double_t,0, "Costmap autoclear rate", 0.1, 0.01, 1.0)
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)
exit(gen.generate(PACKAGE, "ADCNavClient", "ADCNavClient"))
#! /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_ros_tools.dyn_params import add_module_service_params,add_module_action_params,add_module_params
gen = ParameterGenerator()
odom = gen.add_group("Odometry")
tf = gen.add_group("TF")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
add_module_params(gen,"head_module")
move_base_action=add_module_action_params(gen,"move_base")
move_base_action.add("move_base_frame_id",str_t, 0, "Reference frame of the position goals","map")
move_base_action.add("move_base_cancel_prev", bool_t, 0, "Cancel previous action", True)
odom.add("odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50)
tf.add("tf_timeout_time_s", double_t, 0, "Maximum time to wait for transform",5, 0.01, 50)
costmap=add_module_service_params(gen,"clear_costmap")
costmap.add("clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False)
costmap.add("clear_costmap_auto_clear_rate_hz",double_t, 0, "Clear costmaps period", 0.1, 0.01, 1.0)
exit(gen.generate(PACKAGE, "ADCNavModuleAlgorithm", "ADCNavModule"))
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
move_poi_max_retries: 1
move_poi_feedback_watchdog_time_s: 1.0
move_poi_enable_watchdog: True
move_poi_timeout_s: 100.0
move_poi_enable_timeout: True
move_poi_enabled: True
move_waypoint_max_retries: 1
move_waypoint_feedback_watchdog_time_s: 1.0
move_waypoint_enable_watchdog: True
move_waypoint_timeout_s: 100.0
move_waypoint_enable_timeout: True
move_waypoint_enabled: True
odom_watchdog_time_s: 1.0
tf_timeout_time_s: 5.0
clear_costmaps_max_retries: 1.0
clear_costmaps_enable_auto_clear: False
clear_costmaps_auto_clear_rate_hz: 0.1
clear_costmaps_enabled: True
change_map_max_retries: 1
change_map_enabled: True
set_op_mode_max_retries: 1
set_op_mode_enabled: True
// 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
// 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 <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 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
// 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 <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
This diff is collapsed.
#ifndef _ADC_NAV_MODULE_H
#define _ADC_NAV_MODULE_H
// IRI ROS headers
#include <iri_ros_tools/module.h>
#include <iri_ros_tools/module_action.h>
#include <iri_ros_tools/module_service.h>
#include <iri_ros_tools/module_dyn_reconf.h>
#include <iri_ros_tools/watchdog.h>
#include <iri_ros_tools/timeout.h>
#include <iostream>
#include <limits>
// ROS headers
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_listener.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <std_srvs/Empty.h>
// Dynamic reconfigure header
#include <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 Navigation module
*
* This module wraps the basic features of the mobile base navigation of the
* ADC robot. It provides the following features:
* * Navigation to a cartesian point (x,y,theta)
* * Navigation to a point of interest
* * Navigation through a set of waypoints
*
* The current goal can be canceled at any time. If a new goal is given before
* the previous goals has finished, the previous goal is preempted and the new
* goal executed.
*
* Before accepting any goal, this module tries to change the navigation mode
* to LOC. If unsuccessfull, it will not accept any goals. However, if the
* mode is changed afterwards, this module will be unable to detect it.
*
* This module also allows to clear the costmaps when desired or at regular
* intervals to facilitate the navigation, and choose the desired map.
*/
class CADCNavModule : public CModule<adc_nav_module::ADCNavModuleConfig>
{
private:
/**
* \brief
*
*/
adc_nav_module::ADCNavModuleConfig config;
// basic navigation action
/**
* \brief
*
*/
CModuleAction<move_base_msgs::MoveBaseAction,adc_nav_module::ADCNavModuleConfig> move_base_action;
/**
* \brief
*
*/
move_base_msgs::MoveBaseGoal move_base_goal;
/**
* \brief
*
*/
std::string goal_frame_id;
/**
* \brief
*
*/
bool new_goal;
/**
* \brief
*
*/
CModuleService<nav_msgs::GetPlan,adc_nav_module::ADCNavModuleConfig> make_plan;
// dynamic reconfigure
/**
* \brief
*
*/
CModuleDynReconf<adc_nav_module::ADCNavModuleConfig> move_base_reconf;
// odometry
/**
* \brief
*
*/
CROSWatchdog odom_watchdog;
/**
* \brief
*
*/
ros::Subscriber odom_subscriber;
/**
* \brief
*
*/
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
/**
* \brief
*
*/
ros::Subscriber path_subscriber;
/**
* \brief
*
*/
void path_callback(const nav_msgs::Path::ConstPtr& msg);
/**
* \brief
*
*/
nav_msgs::Path path_msg;
/**
* \brief
*
*/
bool path_available;
/**
* \brief
*
*/
geometry_msgs::PoseStamped current_pose;
// tf listener
/**
* \brief
*
*/
tf::TransformListener listener;
/**
* \brief
*
*/
bool transform_position;
/**
* \brief
*
*/
bool transform_orientation;
/**
* \brief
*
*/
CROSTimeout tf_timeout;
// costmaps
/**
* \brief
*
*/
CModuleService<std_srvs::Empty,adc_nav_module::ADCNavModuleConfig> clear_costmaps;
/**
* \brief
*
*/
ros::Timer clear_costmaps_timer;
/**
* \brief
*
*/
void clear_costmaps_call(const ros::TimerEvent& event);
/**
* \brief
*
*/
bool enable_auto_clear;
//Variables
/**
* \brief
*
*/
adc_nav_module_state_t state;
/**
* \brief
*
*/
adc_nav_module_status_t status;
/**
* \brief
*
*/
bool cancel_pending;
protected:
/**
* \brief
*
*/
void state_machine(void);
/**
* \brief
*
*/
void reconfigure_callback(adc_nav_module::ADCNavModuleConfig &config, uint32_t level);
/**
* \brief
*
*/
bool transform_current_pose(void);
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
*
*/
bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,double tolerance=DEFAULT_X_Y_POS_TOL);
// common functions
/**
* \brief Set the goal reference frame
*
* This functions sets the reference with used for all the position,
* orientation and pose goals. By default this frame is set to /map.
*
* \param frame_id name of the new reference frame for the future goals.
* This frame can be any that exist inside the TF tree
* of the robot.
*/
void set_goal_frame(std::string &frame_id);
/**
* \brief Stops the current goal
*
* 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);
// costmaps functions
/**
* \brief Clears the costmaps
*
* This functions can be used to clear the navigation costmapas at any time.
* On success, this function will remove any ghost obstacles that remained
* from previous detections. Calling this function when the auto-clearing
* feature is enabled will have no effect.
*
* Clearing the costmaps may improve the robot's navigation in narrow spaces.
*
* \return a boolean indicating whether the costmaps have been cleared (true)
* or not (false);
*/
bool costmaps_clear(void);
/**
* \brief Enable the autoclear of the costmaps
*
* This functions enables a periodic clearing of the navigation costmaps at a
* desired rate, which in general should be lower than 1 Hz. When this feature
* is enabled, it will be no longer possible to clear the costmaps manually
* with the costmaps_clear() function.
*
* \param rate_hz the desired clearing rate in Hz. This value should be less
* than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
*
*/
void costmaps_enable_auto_clear(double rate_hz);
/**
* \brief Disable the autoclear feature
*
* This function disables the periodic clearing of the navigation costmaps.
* After disabling this feature, the costmaps_clear() function may be used
* to do so.
*/
void costmaps_disable_auto_clear(void);
/**
* \brief Checks the status of the autoclear feature
*
* This functions checks whether the costmaos autoclear feature is enabled
* or not.
*
* \return a boolean indicating whether the costmaps autoclear feature is
* enabled (true) or not (false).
*/
bool costamp_is_auto_clear_enabled(void);
/**
* \brief Function to get the current pose of the robot.
*
* \return The robot pose with respect to the map frame.
*/
geometry_msgs::Pose getCurrentPose(void);
/**
* \brief Function to get the euclidian distance to the goal.
*
* \return The Euclidian distance to the goal. -1.0 when not calculated.
*/
double getGoalDistance(void);
/* parameter functions */
/**
* \brief
*
*/
bool are_costmaps_shutdown(void);
/**
* \brief
*
*/
void shutdown_costmaps(bool shutdown);
/**
* \brief
*
*/
unsigned int get_max_num_retries(void);
/**
* \brief
*
*/
void set_max_num_retries(unsigned int retries);
/**
* \brief
*
*/
double get_planner_frequency(void);
/**
* \brief
*
*/
void set_planner_frequency(double freq);
/**
* \brief
*
*/
double get_planner_patience(void);
/**
* \brief
*
*/
void set_planner_patience(double time);
/**
* \brief
*
*/
double get_controller_frequency(void);
/**
* \brief
*
*/
void set_controller_frequency(double freq);
/**
* \brief
*
*/
double get_controller_patience(void);
/**
* \brief
*
*/
void set_controller_patience(double time);
/**
* \brief Destructor
*
*/
~CADCNavModule();
};
#endif
#ifndef _ADC_NAV_COSTMAP_MODULE_H
#define _ADC_NAV_COSTMAP_MODULE_H
// IRI ROS headers
#include <iri_ros_tools/module_dyn_reconf.h>
// Dynamic reconfigure header
#include <adc_nav_module/ADCNavModuleConfig.h>
/**
* \brief
*
*/
template <class ModuleCfg>
class CADCNavCostmapModule
{
private:
// dynamic reconfigure
/**
* \brief
*
*/
CModuleDynReconf<ModuleCfg> costmap_reconf;
public:
/**
* \brief Constructor
*
*/
CADCNavCostmapModule(const std::string &name,const std::string &name_space=std::string(""));
/* parameter functions */
/**
* \brief
*
*/
unsigned int get_width(void);
/**
* \brief
*
*/
void set_width(unsigned int width);
/**
* \brief
*
*/
unsigned int get_height(void);
/**
* \brief
*
*/
void set_height(unsigned int height);
/**
* \brief
*
*/
void get_footprint(std::vector<std_msgs::Point> &footprint);
/**
* \brief
*
*/
void set_footprint(std::vector<std_msgs::Point> &footprint);
/**
* \brief
*
*/
double get_transform_tolerance(void);
/**
* \brief
*
*/
void set_transform_tolerance(double tol);
/**
* \brief
*
*/
double get_update_frequency(void);
/**
* \brief
*
*/
void set_update_frequency(double freq);
/**
* \brief
*
*/
double get_publish_frequency(void);
/**
* \brief
*
*/
void set_publish_frequency(double freq);
/**
* \brief
*
*/
double get_resolution(void);
/**
* \brief
*
*/
void set_resolution(double res);
/**
* \brief
*
*/
void get_origin(double &x,double &y);
/**
* \brief
*
*/
void set_origin(double &y,double &y);
/**
* \brief
*
*/
double get_footprint_padding(void);
/**
* \brief
*
*/
void set_footprint_padding(double pad);
/**
* \brief Destructor
*
*/
~CADCNavModule();
};
#endif
#ifndef _NAV_MODULE_H
#define _NAV_MODULE_H
// IRI ROS headers
#include <iri_ros_tools/module.h>
#include <iri_ros_tools/module_action.h>
#include <iri_ros_tools/module_service.h>
#include <iri_ros_tools/module_dyn_reconf.h>
#include <iri_ros_tools/watchdog.h>
#include <iri_ros_tools/timeout.h>
#include <iostream>
#include <limits>
// ROS headers
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/transform_listener.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <std_srvs/Empty.h>
//States
typedef enum {NAV_MODULE_IDLE,
NAV_MODULE_TRANSFORM,
NAV_MODULE_START,
NAV_MODULE_WAIT} nav_module_state_t;
//Status
/**
* \brief Possible module status returned by the get_status() function
*
* These are the possible status of the module, which are:
* * NAV_MODULE_RUNNING: indicates that the module is active and operating
* properly.
* * NAV_MODULE_SUCCESS: indicates the last navigation goal has been reached
* successfully.
* * NAV_MODULE_ACTION_START_FAIL: indicates that the goal could not be started.
* * NAV_MODULE_TIMEOUT: indicates that the goal did not complete in the
* allowed time.
* * NAV_MODULE_FB_WATCHDOG: indicates that feedback has not been received for
* a long time.
* * NAV_MODULE_ABORTED: indicates that the navigation goal could not be reached.
* * NAV_MODULE_PREEMPTED: indicates that the current navigation goal has been
* canceled by another goal.
* * NAV_MODULE_REJECTED: indicates that the goal informatio is not valid and the
* goal will not be executed.
* * NAV_MODULE_SET_PARAM_FAIL: indicates that it was impossible to set the value
* of a parameter.
* * NAV_MODULE_PARAM_NOT_PRESENT: indicates that the parameter to be changed is
* not present in the remote node.
* * NAV_MODULE_NO_ODOM: indicates that no odometry information is being received,
* so it is not possible to get the current position of the
* robot.
* * NAV_MODULE_NO_TRANSFORM: indicates that there exist no transform between the
* desired goal frame and the rest of the TF tree.
*/
typedef enum {NAV_MODULE_RUNNING,
NAV_MODULE_SUCCESS,
NAV_MODULE_ACTION_START_FAIL,
NAV_MODULE_TIMEOUT,
NAV_MODULE_FB_WATCHDOG,
NAV_MODULE_ABORTED,
NAV_MODULE_PREEMPTED,
NAV_MODULE_REJECTED,
NAV_MODULE_SET_PARAM_FAIL,
NAV_MODULE_PARAM_NOT_PRESENT,
NAV_MODULE_NO_ODOM,
NAV_MODULE_NO_TRANSFORM} nav_module_status_t;
#define DEFAULT_HEADING_TOL -1.0
#define IGNORE_HEADING 3.14159
#define DEFAULT_X_Y_POS_TOL -1.0
#define IGNORE_X_Y_POS 100.0
/**
* \brief Navigation module
*
* This module wraps the basic features of the mobile base navigation of the
* robot. It provides the following features:
* * Navigation to a cartesian point (x,y,theta)
* * Navigation to a point of interest
* * Navigation through a set of waypoints
*
* The current goal can be canceled at any time. If a new goal is given before
* the previous goals has finished, the previous goal is preempted and the new
* goal executed.
*
* Before accepting any goal, this module tries to change the navigation mode
* to LOC. If unsuccessfull, it will not accept any goals. However, if the
* mode is changed afterwards, this module will be unable to detect it.
*
* This module also allows to clear the costmaps when desired or at regular
* intervals to facilitate the navigation, and choose the desired map.
*/
template <class ModuleCfg>
class CNavModule : public CModule<ModuleCfg>
{
private:
/**
* \brief
*
*/
ModuleCfg config;
// basic navigation action
/**
* \brief
*
*/
CModuleAction<move_base_msgs::MoveBaseAction,ModuleCfg> move_base_action;
/**
* \brief
*
*/
move_base_msgs::MoveBaseGoal move_base_goal;
/**
* \brief
*
*/
std::string goal_frame_id;
/**
* \brief
*
*/
bool new_goal;
/**
* \brief
*
*/
CModuleService<nav_msgs::GetPlan,ModuleCfg> make_plan;
// dynamic reconfigure
/**
* \brief
*
*/
CModuleDynReconf<ModuleCfg> move_base_reconf;
// odometry
/**
* \brief
*
*/
CROSWatchdog odom_watchdog;
/**
* \brief
*
*/
ros::Subscriber odom_subscriber;
/**
* \brief
*
*/
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
/**
* \brief
*
*/
ros::Subscriber path_subscriber;
/**
* \brief
*
*/
void path_callback(const nav_msgs::Path::ConstPtr& msg);
/**
* \brief
*
*/
nav_msgs::Path path_msg;
/**
* \brief
*
*/
bool path_available;
/**
* \brief
*
*/
geometry_msgs::PoseStamped current_pose;
// tf listener
/**
* \brief
*
*/
tf::TransformListener listener;
/**
* \brief
*
*/
bool transform_position;
/**
* \brief
*
*/
bool transform_orientation;
/**
* \brief
*
*/
CROSTimeout tf_timeout;
// costmaps
/**
* \brief
*
*/
CModuleService<std_srvs::Empty,ModuleCfg> clear_costmaps;
/**
* \brief
*
*/
ros::Timer clear_costmaps_timer;
/**
* \brief
*
*/
void clear_costmaps_call(const ros::TimerEvent& event);
/**
* \brief
*
*/
bool enable_auto_clear;
//Variables
/**
* \brief
*
*/
nav_module_state_t state;
/**
* \brief
*
*/
nav_module_status_t status;
/**
* \brief
*
*/
bool cancel_pending;
protected:
/**
* \brief
*
*/
void state_machine(void);
/**
* \brief
*
*/
void reconfigure_callback(ModuleCfg &config, uint32_t level)=0;
/**
* \brief
*
*/
bool transform_current_pose(void);
public:
/**
* \brief Constructor
*
*/
CNavModule(const std::string &name,const std::string &name_space=std::string(""));
// 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
*
*/
bool make_plan(double start_x,double start_y,double start_yaw,double end_x,double end_y,double end_yaw,double tolerance=DEFAULT_X_Y_POS_TOL);
// common functions
/**
* \brief Set the goal reference frame
*
* This functions sets the reference with used for all the position,
* orientation and pose goals. By default this frame is set to /map.
*
* \param frame_id name of the new reference frame for the future goals.
* This frame can be any that exist inside the TF tree
* of the robot.
*/
void set_goal_frame(std::string &frame_id);
/**
* \brief Stops the current goal
*
* This functions stops the motion of the robot, whatever the type of goal
* that has been given. This functions signals the stop to the navigation
* module, but the is_finished() function must be used to know when the
* navigation is actually stopped.
*/
void stop(void);
/**
* \brief Checks if the last goal has finished or not.
*
* This function must be used to know when the last navigation goal has
* ended, either successfully or by any error.
*
* \return a boolean indicating whether the last navigation goal has ended
* (true) or not (false), reagardless of its success or not.
*/
bool is_finished(void);
/**
* \brief Returns the current state
*
* This function returns the current status of the navigation module. It returns
* one of the nav_module_status_t values. This can be called at any time, but is
* has only meaning when a goal has just finished.
*
* \return the current status of the module.
*/
nav_module_status_t get_status(void);
// costmaps functions
/**
* \brief Clears the costmaps
*
* This functions can be used to clear the navigation costmapas at any time.
* On success, this function will remove any ghost obstacles that remained
* from previous detections. Calling this function when the auto-clearing
* feature is enabled will have no effect.
*
* Clearing the costmaps may improve the robot's navigation in narrow spaces.
*
* \return a boolean indicating whether the costmaps have been cleared (true)
* or not (false);
*/
bool costmaps_clear(void);
/**
* \brief Enable the autoclear of the costmaps
*
* This functions enables a periodic clearing of the navigation costmaps at a
* desired rate, which in general should be lower than 1 Hz. When this feature
* is enabled, it will be no longer possible to clear the costmaps manually
* with the costmaps_clear() function.
*
* \param rate_hz the desired clearing rate in Hz. This value should be less
* than 1 Hz, normally in the range of 0.1 to 0.01 Hz.
*
*/
void costmaps_enable_auto_clear(double rate_hz);
/**
* \brief Disable the autoclear feature
*
* This function disables the periodic clearing of the navigation costmaps.
* After disabling this feature, the costmaps_clear() function may be used
* to do so.
*/
void costmaps_disable_auto_clear(void);
/**
* \brief Checks the status of the autoclear feature
*
* This functions checks whether the costmaos autoclear feature is enabled
* or not.
*
* \return a boolean indicating whether the costmaps autoclear feature is
* enabled (true) or not (false).
*/
bool costamp_is_auto_clear_enabled(void);
/**
* \brief Function to get the current pose of the robot.
*
* \return The robot pose with respect to the map frame.
*/
geometry_msgs::Pose getCurrentPose(void);
/**
* \brief Function to get the euclidian distance to the goal.
*
* \return The Euclidian distance to the goal. -1.0 when not calculated.
*/
double getGoalDistance(void);
/* parameter functions */
/**
* \brief
*
*/
bool are_costmaps_shutdown(void);
/**
* \brief
*
*/
void shutdown_costmaps(bool shutdown);
/**
* \brief
*
*/
unsigned int get_max_num_retries(void);
/**
* \brief
*
*/
void set_max_num_retries(unsigned int retries);
/**
* \brief
*
*/
double get_planner_frequency(void);
/**
* \brief
*
*/
void set_planner_frequency(double freq);
/**
* \brief
*
*/
double get_planner_patience(void);
/**
* \brief
*
*/
void set_planner_patience(double time);
/**
* \brief
*
*/
double get_controller_frequency(void);
/**
* \brief
*
*/
void set_controller_frequency(double freq);
/**
* \brief
*
*/
double get_controller_patience(void);
/**
* \brief
*
*/
void set_controller_patience(double time);
/**
* \brief Destructor
*
*/
~CNavModule();
};
#endif
#ifndef _ADC_NAV_PLANNER_MODULE_H
#define _ADC_NAV_PLANNER_MODULE_H
// IRI ROS headers
#include <iri_ros_tools/module_dyn_reconf.h>
// Dynamic reconfigure header
#include <adc_nav_module/ADCNavModuleConfig.h>
/**
* \brief
*
*/
template <class ModuleCfg>
class CADCNavPlannerModule
{
private:
// dynamic reconfigure
/**
* \brief
*
*/
CModuleDynReconf<ModuleCfg> planner_reconf;
public:
/**
* \brief Constructor
*
*/
CADCNavPlannerModule(const std::string &name,const std::string &name_space=std::string(""));
/* parameter functions */
/**
* \brief Destructor
*
*/
~CADCNavModule();
};
#endif
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="nav_client" />
<arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
<arg name="tree_file" default="bt_test" />
<arg name="bt_client_rate" default="10" />
<include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="path_topic" value="$(arg path_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
<arg name="tree_path" value="$(arg tree_path)"/>
<arg name="tree_file" value="$(arg tree_file)"/>
<arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
</include>
</launch>
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="nav_client" />
<arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
<arg name="tree_file" default="bt_test" />
<arg name="bt_client_rate" default="10" />
<include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
<arg name="public_sim" value="true" />
<arg name="robot" value="steel" />
</include>
<include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="path_topic" value="$(arg path_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
</include>
</launch>
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="nav_client" />
<arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="path_topic" value="$(arg path_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
</include>
</launch>
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="nav_client" />
<arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
<arg name="public_sim" value="true" />
<arg name="robot" value="steel" />
</include>
<include file="$(find tiago_nav_module)/launch/tiago_nav_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="path_topic" value="$(arg path_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
</include>
</launch>
<?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>
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="node_name" default="nav_client" />
<arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="path_topic" default="/move_base/GlobalPlanner/plan"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
<arg name="tree_file" default="bt_test" />
<arg name="bt_client_rate" default="10" />
<include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
<arg name="public_sim" value="true" />
<arg name="robot" value="steel" />
</include>
<include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="path_topic" value="$(arg path_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
<arg name="tree_path" value="$(arg tree_path)"/>
<arg name="tree_file" value="$(arg tree_file)"/>
<arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
</include>
</launch>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment