diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0990695f844632af82ed25efb5f76b441d2a0f39..0e61d8b00288abfd1dd95e3de9c09614b04dc15b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@ project(tiago_modules)
## 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 play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_interaction_msgs pal_waypoint_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -93,6 +93,7 @@ generate_dynamic_reconfigure_options(
cfg/NavModule.cfg
cfg/TTSModule.cfg
cfg/ArmModule.cfg
+ cfg/MoveBaseModule.cfg
)
###################################
@@ -106,7 +107,7 @@ generate_dynamic_reconfigure_options(
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module
+ LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module move_base_module
CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs
# DEPENDS system_lib
@@ -228,6 +229,21 @@ target_link_libraries(arm_module ${iriutils_LIBRARY})
## either from message generation or dynamic reconfigure
add_dependencies(arm_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+## Declare a C++ library
+add_library(move_base_module
+ src/move_base_module.cpp
+)
+
+target_link_libraries(move_base_module ${catkin_LIBRARIES})
+target_link_libraries(move_base_module ${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(move_base_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
diff --git a/cfg/MoveBaseModule.cfg b/cfg/MoveBaseModule.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..78a95c763ffd530003561082a50051a8ed347c86
--- /dev/null
+++ b/cfg/MoveBaseModule.cfg
@@ -0,0 +1,48 @@
+#! /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='move_base_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# 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)
+gen.add("rate_hz", double_t, 0, "Move base module rate in Hz", 1.0, 1.0, 100.0)
+gen.add("cmd_vel_rate_hz", double_t, 0, "Cmd vel publish rate in Hz", 10.0, 1.0, 100.0)
+gen.add("distance_tol", double_t, 0, "Tolerancec for the distance", 0.05, 0.001, 0.5)
+gen.add("velocity", double_t, 0, "Velocity", 0.2, 0.1, 0.8)
+
+
+exit(gen.generate(PACKAGE, "MoveBaseModuleAlgorithm", "MoveBaseModule"))
diff --git a/config/move_base_module_default.yaml b/config/move_base_module_default.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..edee7b056eb80b16b564207a34066adbf7bc2b04
--- /dev/null
+++ b/config/move_base_module_default.yaml
@@ -0,0 +1,4 @@
+rate_hz: 1.0
+cmd_vel_rate_hz: 10.0
+distance_tol: 0.05
+velocity: 0.2
diff --git a/include/tiago_modules/move_base_module.h b/include/tiago_modules/move_base_module.h
new file mode 100644
index 0000000000000000000000000000000000000000..f542fdca667d4c7ec25c568d2b5bd998737cf46b
--- /dev/null
+++ b/include/tiago_modules/move_base_module.h
@@ -0,0 +1,77 @@
+#ifndef _MOVE_BASE_MODULE_H
+#define _MOVE_BASE_MODULE_H
+
+#include <iri_ros_tools/module.h>
+#include <iri_ros_tools/module_action.h>
+
+#include <tiago_modules/MoveBaseModuleConfig.h>
+
+//Action
+#include <moveit_msgs/MoveGroupAction.h>
+#include <moveit_msgs/GetPlanningScene.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/QuaternionStamped.h>
+#include <sensor_msgs/JointState.h>
+
+#include <cmath>
+#include <ros/timer.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+
+//States
+typedef enum {MOVE_BASE_MODULE_IDLE, MOVE_BASE_MODULE_START, MOVE_BASE_MODULE_WAIT} move_base_module_state_t;
+
+//Status
+typedef enum {MOVE_BASE_MODULE_RUNNING,
+ MOVE_BASE_MODULE_SUCCESS,
+ MOVE_BASE_MODULE_STOPPED} move_base_module_status_t;
+
+
+class CMoveBaseModule : public CModule<move_base_module::MoveBaseModuleConfig>
+{
+ private:
+
+ move_base_module::MoveBaseModuleConfig config;
+
+ // odometry
+ CROSWatchdog joint_states_watchdog;
+ ros::Subscriber odom_subscriber;
+ nav_msgs::Odometry target_odom;
+ geometry_msgs::Pose goal;
+ void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+
+ //Variables
+ move_base_module_state_t state;
+ move_base_module_status_t status;
+ bool new_position;
+ bool cancel_pending;
+
+ // cmd vel publisher
+ ros::Publisher cmd_vel_publisher;
+ geometry_msgs::Twist cmd_vel_goal;
+ ros::Timer cmd_vel_timer;
+
+ double distance_tol;
+ double target;
+ bool target_achieved;
+ double velocity;
+
+ protected:
+ void state_machine(void);
+ void reconfigure_callback(move_base_module::MoveBaseModuleConfig &config, uint32_t level);
+ void cmd_vel_pub(const ros::TimerEvent& event);
+
+ public:
+ CMoveBaseModule(const std::string &name,const std::string &name_space=std::string(""));
+ // general functions
+ void move_base(double distance);
+ void get_current_pose(geometry_msgs::PoseStamped &pose);
+ void stop(void);
+ bool is_finished(void);
+ move_base_module_status_t get_status(void);
+
+ ~CMoveBaseModule();
+ };
+
+ #endif
diff --git a/src/move_base_module.cpp b/src/move_base_module.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0bf0b9b5cb56b74f9cbb620439e72bfc448aae87
--- /dev/null
+++ b/src/move_base_module.cpp
@@ -0,0 +1,152 @@
+#include <tiago_modules/move_base_module.h>
+
+CMoveBaseModule::CMoveBaseModule(const std::string &name,const std::string &name_space) : CModule(name,name_space)
+{
+ this->start_operation();
+
+ // initialize odometry subscriber
+ this->odom_subscriber = this->module_nh.subscribe("/mobile_base_controller/odom",1,&CMoveBaseModule::odom_callback,this);
+ //Variables
+ this->state=MOVE_BASE_MODULE_IDLE;
+ this->status=MOVE_BASE_MODULE_SUCCESS;
+ this->new_position=false;
+ this->cancel_pending=false;
+ this->target_achieved=false;
+ this->distance_tol=0.05;
+ this->target=0.0;
+ this->velocity=0.2;
+
+ this->cmd_vel_publisher = this->module_nh.advertise<geometry_msgs::Twist>("/mobile_base_controller/cmd_vel", 1);
+ this->cmd_vel_goal.linear.x=0.0;
+ this->cmd_vel_goal.linear.y=0.0;
+ this->cmd_vel_goal.linear.z=0.0;
+ this->cmd_vel_goal.angular.x=0.0;
+ this->cmd_vel_goal.angular.y=0.0;
+ this->cmd_vel_goal.angular.z=0.0;
+ this->cmd_vel_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.cmd_vel_rate_hz),&CMoveBaseModule::cmd_vel_pub,this);
+ this->cmd_vel_timer.stop();
+}
+
+/* State machine */
+void CMoveBaseModule::state_machine(void)
+{
+ switch(this->state)
+ {
+ case MOVE_BASE_MODULE_IDLE: ROS_DEBUG("CMoveBaseModule: Idle");
+ if(this->new_position)
+ {
+ this->new_position=false;
+ this->state=MOVE_BASE_MODULE_START;
+ }
+ else
+ this->state=MOVE_BASE_MODULE_IDLE;
+ break;
+
+ case MOVE_BASE_MODULE_START: ROS_DEBUG("CMoveBaseModule: Start");
+ this->cmd_vel_timer.start();
+ this->state=MOVE_BASE_MODULE_WAIT;
+ break;
+
+ case MOVE_BASE_MODULE_WAIT: ROS_DEBUG("CMoveBaseModule: Move base");
+ if(this->target_achieved)
+ {
+ this->cmd_vel_timer.stop();
+ this->target_achieved=false;
+ this->state=MOVE_BASE_MODULE_IDLE;
+ this->status=MOVE_BASE_MODULE_SUCCESS;
+ }
+ else if(this->cancel_pending)
+ {
+ this->cmd_vel_timer.stop();
+ this->cancel_pending=false;
+ this->state=MOVE_BASE_MODULE_IDLE;
+ this->status=MOVE_BASE_MODULE_STOPPED;
+ }
+ else
+ {
+ this->state=MOVE_BASE_MODULE_WAIT;
+ }
+ break;
+
+ }
+}
+
+void CMoveBaseModule::reconfigure_callback(move_base_module::MoveBaseModuleConfig &config, uint32_t level)
+{
+ ROS_DEBUG("CMoveBaseModule : reconfigure callback");
+ this->lock();
+ this->config=config;
+ /* set the module rate */
+ this->set_rate(config.rate_hz);
+ // set parameters
+ this->distance_tol=config.distance_tol;
+ this->velocity=config.velocity;
+
+ this->unlock();
+}
+
+void CMoveBaseModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ ROS_DEBUG("CMoveBaseModule: Odometry callback");
+ double init_odom_rot;
+ double difference;
+
+ this->lock();
+ if(new_position)
+ {
+ this->target_odom.pose.pose.position.x = msg->pose.pose.position.x + this->target;
+ }
+ else if(this->state!=MOVE_BASE_MODULE_IDLE)
+ {
+ difference = std::abs(this->target_odom.pose.pose.position.x - msg->pose.pose.position.x);
+ if(difference < this->distance_tol)
+ {
+ this->target_achieved=true;
+ }
+ }
+ this->unlock();
+}
+
+void CMoveBaseModule::cmd_vel_pub(const ros::TimerEvent& event)
+{
+ ROS_DEBUG("CMoveBaseModule: Cmd_vel pusblish");
+ this->cmd_vel_publisher.publish(this->cmd_vel_goal);
+}
+
+// pose motion
+void CMoveBaseModule::move_base(double distance)
+{
+ this->lock();
+ if(!this->is_finished())
+ this->stop();
+ this->new_position=true;
+ if(distance>0)
+ this->cmd_vel_goal.linear.x=this->velocity;
+ else
+ this->cmd_vel_goal.linear.x=-this->velocity;
+ this->target=distance;
+ this->unlock();
+}
+
+void CMoveBaseModule::stop(void)
+{
+ if(this->state!=MOVE_BASE_MODULE_IDLE)
+ this->cancel_pending=true;
+}
+
+bool CMoveBaseModule::is_finished(void)
+{
+ if(this->state==MOVE_BASE_MODULE_IDLE && this->new_position==false)
+ return true;
+ else
+ return false;
+}
+
+move_base_module_status_t CMoveBaseModule::get_status(void)
+{
+ return this->status;
+}
+
+CMoveBaseModule::~CMoveBaseModule()
+{
+}