From 3efc57bcf5ebdfb63af054ba291ea297ab6a1bba Mon Sep 17 00:00:00 2001
From: Pioneer3 <lab1_5@P3-DX-4322.(none)>
Date: Fri, 28 Sep 2018 16:51:35 +0200
Subject: [PATCH] Added function to publish robot_info. Also adjusted stop
 handling

---
 RosAria.cpp | 42 ++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 40 insertions(+), 2 deletions(-)

diff --git a/RosAria.cpp b/RosAria.cpp
index 1baf97a..6978c53 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -34,7 +34,7 @@
 
 
 #include <sstream>
-
+#include <cstdlib>
 
 // Node that interfaces between ROS and mobile robot base features via ARIA library. 
 //
@@ -60,6 +60,7 @@ public:
     //void cmd_disable_motors_cb();
     void spin();
     void publish();
+    void publish_robot_info();
     void sonarConnectCb();
     void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
     void readParameters();
@@ -85,14 +86,19 @@ protected:
 
     ros::Subscriber cmdvel_sub;
 
-    // Olek new subscribers
+    // Olek new subscribers and publishers
     ros::Subscriber restrictions_sub;
     ros::Subscriber userstop_sub;
     ros::Subscriber masterstop_sub;
     ros::Subscriber clutch_sub;
 
+    ros::Publisher robot_info_pub;
+
+    // true means robot is running, false means robot is stopped
+    bool robot_state;
     bool userstop_state;
     bool masterstop_state;
+
     bool clutch_state;
 
     ros::ServiceServer enable_srv;
@@ -390,6 +396,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
     masterstop_sub = n.subscribe( "/PIONIER/master_stop", 1, &RosAriaNode::masterstop_cb,this);
     clutch_sub = n.subscribe( "clutch", 1, &RosAriaNode::clutch_cb,this);
 
+    robot_info_pub = n.advertise<rosaria_msgs::RobotInfoMsg>("robot_info", 1, true /*latch*/ );
 
     // advertise enable/disable services
     enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
@@ -574,6 +581,23 @@ void RosAriaNode::spin()
     ros::spin();
 }
 
+
+void RosAriaNode::publish_robot_info()
+{
+    rosaria_msgs::RobotInfoMsg robot_info_msg;
+
+    robot_info_msg.robot_id.data = atoi(std::getenv("PIONIER_ID"));
+    robot_info_msg.battery_voltage.data = robot->getRealBatteryVoltageNow();
+    robot_info_msg.twist.linear.x = robot->getVel()/1000;
+    robot_info_msg.twist.linear.y = robot->getLatVel()/1000.0;
+    robot_info_msg.twist.angular.z = robot->getRotVel()*M_PI/180;
+    robot_info_msg.state.data = robot_state;
+    robot_info_msg.clutch.data = clutch_state;
+    // robot_info_msg.header.stamp = ros::Time::now();
+
+    robot_info_pub.publish(robot_info_msg);
+}
+
 void RosAriaNode::publish()
 {
     robot->requestEncoderPackets();
@@ -927,6 +951,12 @@ void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
     {
         // User stop released
         userstop_state = true;
+
+        if (masterstop_state)
+        {
+            // If master stop is not pressed we can set robot in running state
+            robot_state = true;
+        }
     }
     else
     {
@@ -941,6 +971,7 @@ void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
         robot->comInt(ArCommands::ESTOP,0); // ?
         robot->unlock();
 
+        robot_state = false;
         userstop_state = false;
     }
 }
@@ -951,6 +982,12 @@ void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
     {
         // Master stop released
         masterstop_state = true;
+
+        if (userstop_state)
+        {
+            // If user stop is not pressed we can set robot in running state
+            robot_state = true;
+        }
     }
     else
     {
@@ -965,6 +1002,7 @@ void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
         robot->comInt(ArCommands::ESTOP,0); // ?
         robot->unlock();
         
+        robot_state = false;
         masterstop_state = false;
     }
 }