From 0baeeca94bca6fb5e2fa49191f4771ad029c378b Mon Sep 17 00:00:00 2001
From: Pioneer3 <lab1_5@P3-DX-4322.(none)>
Date: Fri, 28 Sep 2018 19:43:59 +0200
Subject: [PATCH] Added timers to stop robots when connection with either user
 or master is lost. Also started working on sonar obstacle stop

---
 RosAria.cpp | 125 +++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 119 insertions(+), 6 deletions(-)

diff --git a/RosAria.cpp b/RosAria.cpp
index 8b1d299..5e86ef2 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -56,9 +56,11 @@ public:
     void masterstop_cb(const std_msgs::Bool &);
     void clutch_cb(const std_msgs::Bool &);
 
+    bool isCloseToObstacle();
+
     void spin();
     void publish();
-    void publish_robot_info();
+    void publishRobotInfo();
     void sonarConnectCb();
     void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
     void readParameters();
@@ -91,6 +93,14 @@ protected:
     bool robot_state;
     bool userstop_state;
     bool masterstop_state;
+    bool is_near_obstacle;
+
+    // Connection timers
+    ros::Timer user_connection_timer;
+    ros::Timer master_connection_timer;
+
+    void user_connection_lost_cb(const ros::TimerEvent&);
+    void master_connection_lost_cb(const ros::TimerEvent&);
 
     //Gripper
     ros::ServiceServer gripper_open_srv;
@@ -298,7 +308,7 @@ void RosAriaNode::sonarConnectCb()
     }
     else if(!publish_sonar && !publish_sonar_pointcloud2)
     {
-        robot->disableSonar();
+        // robot->disableSonar();
         sonar_enabled = true;
     }
     robot->unlock();
@@ -375,6 +385,12 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
     gripper_up_srv = n.advertiseService("gripper_up", &RosAriaNode::gripper_up_cb, this);
     gripper_down_srv = n.advertiseService("gripper_down", &RosAriaNode::gripper_down_cb, this);
 
+    user_connection_timer = n.createTimer(ros::Duration(1.5), &RosAriaNode::user_connection_lost_cb,this);
+    master_connection_timer = n.createTimer(ros::Duration(1.5), &RosAriaNode::master_connection_lost_cb,this);
+
+    user_connection_timer.stop();
+    master_connection_timer.stop();
+
     maxVel=1.4;
     maxRot=1;
     minDist = 0.7;
@@ -520,6 +536,7 @@ int RosAriaNode::Setup()
     robot_state = false;
     userstop_state = false;
     masterstop_state = false;
+    is_near_obstacle = false;
 
     // Enable the motors
     robot->enableMotors();
@@ -545,7 +562,39 @@ void RosAriaNode::spin()
 }
 
 
-void RosAriaNode::publish_robot_info()
+bool RosAriaNode::isCloseToObstacle()
+{
+    float range;
+
+    robot->enableSonar();
+
+    for (int i = 0; i < robot->getNumSonar(); i++) 
+    {
+        ArSensorReading* reading = NULL;
+        reading = robot->getSonarReading(i);
+        if(!reading) 
+        {
+            ROS_WARN("RosAria: Did not receive a sonar reading.");
+            continue;
+        }
+
+        range = reading->getRange();
+
+        std_msgs::Float32 soc;
+        soc.data = (float)(range/1000.0);
+        state_of_charge_pub.publish(soc);
+
+        if ((float)(range/1000.0) < minDist)   // Readings are in mm
+        {
+            return true;
+        }
+    }
+    // If no obstacles are closer than minDist, everything is OK
+    return false;
+}
+
+
+void RosAriaNode::publishRobotInfo()
 {
     rosaria_msgs::RobotInfoMsg robot_info_msg;
 
@@ -563,6 +612,24 @@ void RosAriaNode::publish_robot_info()
 
 void RosAriaNode::publish()
 {
+    // Safety check
+    // is_near_obstacle = isCloseToObstacle();
+
+    // if (is_near_obstacle)
+    // {
+    //     robot->lock();
+
+    //     robot->setVel(0);
+    //     if(robot->hasLatVel())
+    //         robot->setLatVel(0);
+    //     robot->setRotVel(0);
+
+    //     robot->comInt(ArCommands::ESTOP,0); // ?
+    //     robot->unlock();
+
+    //     robot_state = false;
+    // }
+
     robot->requestEncoderPackets();
     Wheel.header.stamp = ros::Time::now();
     Wheel.name.resize(2);
@@ -737,7 +804,7 @@ void RosAriaNode::publish()
         // ROS_INFO("WATCHDOG %d", WATCHDOG);
     }
 
-    publish_robot_info();
+    publishRobotInfo();
 
 }
 
@@ -783,7 +850,8 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
     veltime = ros::Time::now();
     watchdog= ros::Time::now();
 
-    if(!robot_state)
+    // Ignore command if robot stopped or close to the obstacle
+    if(!robot_state || is_near_obstacle)
     {
         ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
         return;
@@ -837,7 +905,11 @@ void RosAriaNode::restrictions_cb( const rosaria_msgs::RestrictionsMsg &msg)
 }
 
 void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
-{
+{   
+    // Restarting user connection timer
+    user_connection_timer.stop();
+    user_connection_timer.start();
+
     if (msg.data == true)
     {
         // User stop released
@@ -869,6 +941,10 @@ void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
 
 void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
 {
+    // Restarting master connection timer
+    master_connection_timer.stop();
+    master_connection_timer.start();
+
     if (msg.data == true)
     {
         // Master stop released
@@ -917,6 +993,43 @@ void RosAriaNode::clutch_cb(const std_msgs::Bool &msg)
 }
 
 
+void RosAriaNode::user_connection_lost_cb(const ros::TimerEvent&)
+{
+    user_connection_timer.stop();
+
+    robot->lock();
+
+    robot->setVel(0);
+    if(robot->hasLatVel())
+        robot->setLatVel(0);
+    robot->setRotVel(0);
+
+    robot->comInt(ArCommands::ESTOP,0); // ?
+    robot->unlock();
+
+    robot_state = false;
+    userstop_state = false;
+}
+
+
+void RosAriaNode::master_connection_lost_cb(const ros::TimerEvent&)
+{
+    master_connection_timer.stop();
+
+    robot->lock();
+
+    robot->setVel(0);
+    if(robot->hasLatVel())
+        robot->setLatVel(0);
+    robot->setRotVel(0);
+
+    robot->comInt(ArCommands::ESTOP,0); // ?
+    robot->unlock();
+    
+    robot_state = false;
+    masterstop_state = false;
+}
+
 int main( int argc, char** argv )
 {
     ros::init(argc,argv, "RosAria");