diff --git a/src/move_base_module.cpp b/src/move_base_module.cpp deleted file mode 100644 index 388bacbe5dffde918f9d55c2086fe55e674f4956..0000000000000000000000000000000000000000 --- a/src/move_base_module.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#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() -{ -}