Skip to content
Snippets Groups Projects
Commit 28a50c9d authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

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

parent e1f92134
No related branches found
No related tags found
No related merge requests found
#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()
{
}
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