diff --git a/RosAria.cpp b/RosAria.cpp index 6978c53..9568ee1 100644 --- a/RosAria.cpp +++ b/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 *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("battery_voltage", 1000); wheel_pub = n.advertise("wheels",1000); recharge_state_pub = n.advertise("battery_recharge_state", 5, true /*latch*/ ); recharge_state.data = -2; state_of_charge_pub = n.advertise("battery_state_of_charge", 100); - motors_state_pub = n.advertise("motors_state", 5, true /*latch*/ ); - stop_motors_pub = n.advertise("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 ) @@ -398,30 +372,17 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) : robot_info_pub = n.advertise("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;