Deleted old version of subscribing and publishing and added new implementations. System ready for first tests with user and master plugins

RosAria.cpp

@@ -56,8 +56,6 @@ public:
     void masterstop_cb(const std_msgs::Bool &);
     void clutch_cb(const std_msgs::Bool &);
 
-    //void cmd_enable_motors_cb();
-    //void cmd_disable_motors_cb();
     void spin();
     void publish();
     void publish_robot_info();
@@ -73,17 +71,12 @@ protected:
     ros::Publisher bumpers_pub;
     ros::Publisher sonar_pub;
     ros::Publisher sonar_pointcloud2_pub;
-    ros::Publisher voltage_pub;
 
     ros::Publisher recharge_state_pub;
     std_msgs::Int8 recharge_state;
 
     ros::Publisher state_of_charge_pub;
 
-    ros::Publisher motors_state_pub;
-    std_msgs::Bool motors_state;
-    bool published_motors_state;
-
     ros::Subscriber cmdvel_sub;
 
     // Olek new subscribers and publishers
@@ -101,16 +94,6 @@ protected:
 
     bool clutch_state;
 
-    ros::ServiceServer enable_srv;
-    ros::ServiceServer disable_srv;
-    bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-    bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-
-    ros::Publisher stop_motors_pub;
-    ros::ServiceServer stop_motors_srv;
-    ros::ServiceServer start_motors_srv;
-    bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
-    bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 
     //Gripper
     ros::ServiceServer gripper_open_srv;
@@ -170,10 +153,6 @@ protected:
     // dynamic_reconfigure
     dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
 
-    //Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
-    bool stop_robot;
-    std_msgs::Bool stop_motors_msg;
-
     ros::Time watchdog;
 };
 
@@ -375,16 +354,11 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
                                                                   boost::bind(&RosAriaNode::sonarConnectCb, this),
                                                                   boost::bind(&RosAriaNode::sonarConnectCb, this));
 
-    voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
     wheel_pub   = n.advertise<sensor_msgs::JointState>("wheels",1000);
     recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
     recharge_state.data = -2;
     state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
 
-    motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
-    stop_motors_pub = n.advertise<std_msgs::Bool>("stop_motors_state", 5, true /*latch*/ );
-    motors_state.data = false;
-    published_motors_state = false;
 
     // subscribe to services
     cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
@@ -398,30 +372,17 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
 
     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);
-    disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
-
     //Damian Gripper
     gripper_open_srv = n.advertiseService("gripper_open", &RosAriaNode::gripper_open_cb, this);
     gripper_close_srv = n.advertiseService("gripper_close", &RosAriaNode::gripper_close_cb, this);
     gripper_up_srv = n.advertiseService("gripper_up", &RosAriaNode::gripper_up_cb, this);
     gripper_down_srv = n.advertiseService("gripper_down", &RosAriaNode::gripper_down_cb, this);
 
-    //Damian usługi stopu awaryjnego
-    stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
-    start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
-    stop_robot=false;
-    stop_motors_msg.data=false;
-
     maxVel=1.4;
     maxRot=1;
     minDist = 0.7;
 
-
     veltime = ros::Time::now();
-
-
 }
 
 RosAriaNode::~RosAriaNode()
@@ -558,6 +519,13 @@ int RosAriaNode::Setup()
 
     dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
 
+    // Setting flags to initial values
+    robot_state = false;
+    userstop_state = false;
+    masterstop_state = false;
+
+    clutch_state = true;
+
     // Enable the motors
     robot->enableMotors();
 
@@ -684,12 +652,6 @@ void RosAriaNode::publish()
 
     bumpers_pub.publish(bumpers);
 
-    //Publish battery information
-    // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
-    std_msgs::Float64 batteryVoltage;
-    batteryVoltage.data = robot->getRealBatteryVoltageNow();
-    voltage_pub.publish(batteryVoltage);
-
     if(robot->haveStateOfCharge())
     {
         std_msgs::Float32 soc;
@@ -706,24 +668,6 @@ void RosAriaNode::publish()
         recharge_state_pub.publish(recharge_state);
     }
 
-    // publish motors state if changed
-    bool e = robot->areMotorsEnabled();
-    if(e != motors_state.data || !published_motors_state)
-    {
-        ROS_INFO("RosAria: publishing stop motors state %d.", e);
-        motors_state.data = e;
-        motors_state_pub.publish(motors_state);
-        published_motors_state = true;
-    }
-
-    //Publikuje czy zatrzymane silniki
-    if(stop_robot)
-    {
-        ROS_INFO("RosAria: publishing new motors state %d.", stop_motors_msg.data);
-        stop_motors_pub.publish(stop_motors_msg);
-        stop_robot = false;
-    }
-
     // Publish sonar information, if enabled.
     if (publish_sonar || publish_sonar_pointcloud2)
     {
@@ -797,60 +741,11 @@ void RosAriaNode::publish()
         robot->setRotVel(0);
         // ROS_INFO("WATCHDOG %d", WATCHDOG);
     }
+
+    publish_robot_info();
+
 }
 
-bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-{
-    ROS_INFO("RosAria: Enable motors request.");
-    robot->lock();
-    if(robot->isEStopPressed())
-        ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
-    robot->enableMotors();
-    robot->unlock();
-    // todo could wait and see if motors do become enabled, and send a response with an error flag if not
-    return true;
-}
-
-bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-{
-    ROS_INFO("RosAria: Disable motors request.");
-    robot->lock();
-    robot->disableMotors();
-    robot->unlock();
-    // todo could wait and see if motors do become disabled, and send a response with an error flag if not
-    return true;
-}
-
-//Damian Metody odpowiedzialne za awaryjne ztrzymanie silników
-bool RosAriaNode::start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-{
-    stop_robot=true;
-    stop_motors_msg.data=false;
-
-    ROS_INFO("RosAria: Disable motors brakes.");
-    robot->lock();
-    if(robot->isEStopPressed())
-        ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
-    robot->unlock();
-    return true;
-}
-
-bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-{
-    robot->lock();
-    stop_robot=true;
-    stop_motors_msg.data=true;
-    //Zatrzymuje robota
-    robot->setVel(0);
-    if(robot->hasLatVel())
-        robot->setLatVel(0);
-    robot->setRotVel(0);
-
-    robot->comInt(ArCommands::ESTOP,0);
-    ROS_INFO("RosAria: Enable motors brakes.");
-    robot->unlock();
-    return true;
-}
 bool  RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
 {
     robot->lock();
@@ -892,7 +787,8 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
 {
     veltime = ros::Time::now();
     watchdog= ros::Time::now();
-    if(stop_motors_msg.data)
+
+    if(!robot_state)
     {
         ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
         return;