Changed name of file from move_base_module.cpp to move_platform_module.cpp

src/move_base_module.cpp

-#include <tiago_modules/move_platform_module.h>
-
-CMovePlatformModule::CMovePlatformModule(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,&CMovePlatformModule::odom_callback,this);
-  //Variables
-  this->state=MOVE_PLATFORM_MODULE_IDLE;
-  this->status=MOVE_PLATFORM_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),&CMovePlatformModule::cmd_vel_pub,this);
-  this->cmd_vel_timer.stop();
-}
-
-/* State machine */
-void CMovePlatformModule::state_machine(void)
-{
-  switch(this->state)
-  {
-    case MOVE_PLATFORM_MODULE_IDLE: ROS_DEBUG("CMovePlatformModule: Idle");
-                                if(this->new_position)
-                                {
-                                  this->new_position=false;
-                                  this->state=MOVE_PLATFORM_MODULE_START;
-                                }
-                                else
-                                  this->state=MOVE_PLATFORM_MODULE_IDLE;
-                                break;
-
-    case MOVE_PLATFORM_MODULE_START: ROS_DEBUG("CMovePlatformModule: Start");
-                                 this->cmd_vel_timer.start();
-                                 this->state=MOVE_PLATFORM_MODULE_WAIT;
-                                 break;
-
-    case MOVE_PLATFORM_MODULE_WAIT: ROS_DEBUG("CMovePlatformModule: Move platform");
-                                if(this->target_achieved)
-                                {
-                                  this->cmd_vel_timer.stop();
-                                  this->target_achieved=false;
-                                  this->state=MOVE_PLATFORM_MODULE_IDLE;
-                                  this->status=MOVE_PLATFORM_MODULE_SUCCESS;
-                                }
-                                else if(this->cancel_pending)
-                                {
-                                  this->cmd_vel_timer.stop();
-                                  this->cancel_pending=false;
-                                  this->state=MOVE_PLATFORM_MODULE_IDLE;
-                                  this->status=MOVE_PLATFORM_MODULE_STOPPED;
-                                }
-                                else
-                                {
-                                  this->state=MOVE_PLATFORM_MODULE_WAIT;
-                                }
-                                break;
-
-  }
-}
-
-void CMovePlatformModule::reconfigure_callback(move_platform_module::MovePlatformModuleConfig &config, uint32_t level)
-{
-  ROS_DEBUG("CMovePlatformModule : 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 CMovePlatformModule::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
-{
-  ROS_DEBUG("CMovePlatformModule: 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_PLATFORM_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 CMovePlatformModule::cmd_vel_pub(const ros::TimerEvent& event)
-{
-  ROS_DEBUG("CMovePlatformModule: Cmd_vel pusblish");
-  this->cmd_vel_publisher.publish(this->cmd_vel_goal);
-}
-
-// pose motion
-void CMovePlatformModule::move_platform(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 CMovePlatformModule::stop(void)
-{
-  if(this->state!=MOVE_PLATFORM_MODULE_IDLE)
-    this->cancel_pending=true;
-}
-
-bool CMovePlatformModule::is_finished(void)
-{
-  if(this->state==MOVE_PLATFORM_MODULE_IDLE && this->new_position==false)
-    return true;
-  else
-    return false;
-}
-
-move_platform_module_status_t CMovePlatformModule::get_status(void)
-{
-  return this->status;
-}
-
-CMovePlatformModule::~CMovePlatformModule()
-{
-}