L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Addded subscribers and methods to handle userstop, masterstop and clutch switching. Not yet used in main program

Browse Source
This commit is contained in:
Pioneer3 2018-09-28 16:20:22 +02:00
parent 4655f91870
commit 8ae1f486c8
1 changed files with 92 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
RosAria.cpp
View File

@ -49,7 +49,12 @@ public:
public:
int Setup();
void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
// Olek new subscribers
void restrictions_cb( const rosaria_msgs::RestrictionsMsg &);
void userstop_cb(const std_msgs::Bool &);
void masterstop_cb(const std_msgs::Bool &);
void clutch_cb(const std_msgs::Bool &);
//void cmd_enable_motors_cb();
//void cmd_disable_motors_cb();
@ -79,7 +84,16 @@ protected:
bool published_motors_state;
ros::Subscriber cmdvel_sub;
// Olek new subscribers
ros::Subscriber restrictions_sub;
ros::Subscriber userstop_sub;
ros::Subscriber masterstop_sub;
ros::Subscriber clutch_sub;
bool userstop_state;
bool masterstop_state;
bool clutch_state;
ros::ServiceServer enable_srv;
ros::ServiceServer disable_srv;
@ -110,7 +124,7 @@ protected:
float maxVel;
float maxRot;
float minDist;
float minDist;
ArRobotConnector *conn;
ArRobot *robot;
@ -370,7 +384,12 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// Olek new subscribers
restrictions_sub = n.subscribe( "/PIONIER/restrictions", 1, &RosAriaNode::restrictions_cb,this);
userstop_sub = n.subscribe( "user_stop", 1, &RosAriaNode::userstop_cb,this);
masterstop_sub = n.subscribe( "/PIONIER/master_stop", 1, &RosAriaNode::masterstop_cb,this);
clutch_sub = n.subscribe( "clutch", 1, &RosAriaNode::clutch_cb,this);
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
@ -390,7 +409,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
maxVel=1.4;
maxRot=1;
minDist = 0.7;
minDist = 0.7;
veltime = ros::Time::now();
@ -684,8 +703,8 @@ void RosAriaNode::publish()
// Publish sonar information, if enabled.
if (publish_sonar || publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
@ -892,7 +911,7 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
(double) x * 1e3, (double) y * 1.3, (double) z * 180/M_PI);
}
// Olek new subscribers
void RosAriaNode::restrictions_cb( const rosaria_msgs::RestrictionsMsg &msg)
{
veltime = ros::Time::now();
@ -902,6 +921,74 @@ void RosAriaNode::restrictions_cb( const rosaria_msgs::RestrictionsMsg &msg)
ROS_INFO( "new max speed: [%0.2f,%0.2f]", maxVel, maxRot);
}
void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
{
if (msg.data == true)
{
// User stop released
userstop_state = true;
}
else
{
// User stop requested
robot->lock();
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
robot->comInt(ArCommands::ESTOP,0); // ?
robot->unlock();
userstop_state = false;
}
}
void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
{
if (msg.data == true)
{
// Master stop released
masterstop_state = true;
}
else
{
// Master stop requested
robot->lock();
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
robot->comInt(ArCommands::ESTOP,0); // ?
robot->unlock();
masterstop_state = false;
}
}
void RosAriaNode::clutch_cb(const std_msgs::Bool &msg)
{
if (msg.data == true)
{
// Engaging clutch
robot->lock();
robot->enableMotors();
robot->unlock();
clutch_state = true;
}
else
{
// Disengaging clutch
robot->lock();
robot->disableMotors();
robot->unlock();
clutch_state = false;
}
}
int main( int argc, char** argv )
{