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

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

parent d3393434
No related branches found
No related tags found
No related merge requests found
#ifndef _MOVE_PLATFORM_MODULE_H
#define _MOVE_PLATFORM_MODULE_H
#include <iri_ros_tools/module.h>
#include <iri_ros_tools/module_action.h>
#include <tiago_modules/MovePlatformModuleConfig.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_PLATFORM_MODULE_IDLE, MOVE_PLATFORM_MODULE_START, MOVE_PLATFORM_MODULE_WAIT} move_platform_module_state_t;
//Status
typedef enum {MOVE_PLATFORM_MODULE_RUNNING,
MOVE_PLATFORM_MODULE_SUCCESS,
MOVE_PLATFORM_MODULE_STOPPED} move_platform_module_status_t;
class CMovePlatformModule : public CModule<move_platform_module::MovePlatformModuleConfig>
{
private:
move_platform_module::MovePlatformModuleConfig 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_platform_module_state_t state;
move_platform_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_platform_module::MovePlatformModuleConfig &config, uint32_t level);
void cmd_vel_pub(const ros::TimerEvent& event);
public:
CMovePlatformModule(const std::string &name,const std::string &name_space=std::string(""));
// general functions
void move_platform(double distance);
void get_current_pose(geometry_msgs::PoseStamped &pose);
void stop(void);
bool is_finished(void);
move_platform_module_status_t get_status(void);
~CMovePlatformModule();
};
#endif
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