From 8ae1f486c876913f6c83fcb57f9a948ddfb81633 Mon Sep 17 00:00:00 2001
From: Pioneer3 <lab1_5@P3-DX-4322.(none)>
Date: Fri, 28 Sep 2018 16:20:22 +0200
Subject: [PATCH] Addded subscribers and methods to handle userstop, masterstop
 and clutch switching. Not yet used in main program

---
 RosAria.cpp | 97 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 92 insertions(+), 5 deletions(-)

diff --git a/RosAria.cpp b/RosAria.cpp
index aa108d5..1baf97a 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -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 )
 {