From 4655f91870226dbedbb267e1beaa7847fbd87ef7 Mon Sep 17 00:00:00 2001
From: Pioneer3 <lab1_5@P3-DX-4322.(none)>
Date: Fri, 28 Sep 2018 15:10:17 +0200
Subject: [PATCH] Removed maxspeed_cb, maxspped_sub and added restrictions_cb,
 restrictions_sub to handle new type of messages

---
 RosAria.cpp | 27 ++++++++++++++++-----------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/RosAria.cpp b/RosAria.cpp
index c0a56e9..aa108d5 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -29,6 +29,10 @@
 #include "std_srvs/Empty.h"
 #include "sensor_msgs/JointState.h"
 
+#include "rosaria_msgs/RestrictionsMsg.h"
+#include "rosaria_msgs/RobotInfoMsg.h"
+
+
 #include <sstream>
 
 
@@ -45,9 +49,7 @@ public:
 public:
     int Setup();
     void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
-    void maxspeed_cb( const geometry_msgs::Vector3ConstPtr &);
-
-
+    void restrictions_cb( const rosaria_msgs::RestrictionsMsg &);
 
     //void cmd_enable_motors_cb();
     //void cmd_disable_motors_cb();
@@ -77,7 +79,7 @@ protected:
     bool published_motors_state;
 
     ros::Subscriber cmdvel_sub;
-    ros::Subscriber maxspeed_sub;
+    ros::Subscriber restrictions_sub;
 
     ros::ServiceServer enable_srv;
     ros::ServiceServer disable_srv;
@@ -108,6 +110,7 @@ protected:
 
     float maxVel;
     float maxRot;
+float minDist;
 
     ArRobotConnector *conn;
     ArRobot *robot;
@@ -367,9 +370,7 @@ 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 ));
 
-
-    maxspeed_sub = n.subscribe( "/PIONIER/distance", 1, (boost::function <void(const geometry_msgs::Vector3ConstPtr&)>)
-                              boost::bind(&RosAriaNode::maxspeed_cb, this, _1 ));
+    restrictions_sub = n.subscribe( "/PIONIER/restrictions", 1, &RosAriaNode::restrictions_cb,this);
 
     // advertise enable/disable services
     enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
@@ -389,6 +390,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
 
     maxVel=1.4;
     maxRot=1;
+minDist = 0.7;
 
 
     veltime = ros::Time::now();
@@ -890,14 +892,17 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
               (double) x * 1e3, (double) y * 1.3, (double) z * 180/M_PI);
 }
 
-void RosAriaNode::maxspeed_cb( const geometry_msgs::Vector3ConstPtr &msg)
+
+void RosAriaNode::restrictions_cb( const rosaria_msgs::RestrictionsMsg &msg)
 {
     veltime = ros::Time::now();
-    maxVel=msg->y;
-    maxRot=msg->z;
-    ROS_INFO( "new max speed: [%0.2f,%0.2f]", msg->y, msg->z);
+    maxVel=msg.linear_velocity.data;
+    maxRot=msg.angular_velocity.data;
+    minDist=msg.distance.data;
+    ROS_INFO( "new max speed: [%0.2f,%0.2f]", maxVel, maxRot);
 }
 
+
 int main( int argc, char** argv )
 {
     ros::init(argc,argv, "RosAria");