Initial implementation of the move base module

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

cfg/MoveBaseModule.cfg

+#! /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"))

config/move_base_module_default.yaml

+rate_hz: 1.0
+cmd_vel_rate_hz: 10.0
+distance_tol: 0.05
+velocity: 0.2

include/tiago_modules/move_base_module.h

+#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

src/move_base_module.cpp

+#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()
+{
+}