L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Dodałem WatchDog

Browse Source
This commit is contained in:
Damian Barański 2015-02-20 13:30:43 +01:00
parent ec1ab02895
commit dba25d430b
1 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
RosAria.cpp
View File

@ -1,3 +1,4 @@
//#define WATCHDOG 1
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#ifdef ADEPT_PKG #ifdef ADEPT_PKG
@ -52,6 +53,7 @@ class RosAriaNode
void readParameters(); void readParameters();
protected: protected:
int WATCHDOG;
ros::NodeHandle n; ros::NodeHandle n;
ros::Publisher pose_pub; ros::Publisher pose_pub;
ros::Publisher bumpers_pub; ros::Publisher bumpers_pub;
@ -124,6 +126,8 @@ class RosAriaNode
//Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie //Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
bool stop_robot; bool stop_robot;
std_msgs::Bool stop_motors_msg; std_msgs::Bool stop_motors_msg;
ros::Time watchdog;
}; };
void RosAriaNode::readParameters() void RosAriaNode::readParameters()
@ -169,6 +173,16 @@ void RosAriaNode::readParameters()
n_.setParam( "RevCount", RevCount); n_.setParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount); ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount);
} }
if (n_.hasParam("WatchDog"))
{
n_.getParam( "WatchDog", WATCHDOG);
ROS_INFO("Setting WatchDog from ROS Parameter: %d", WATCHDOG);
}
else
{
n_.setParam( "WatchDog", WATCHDOG);
ROS_INFO("Setting default WatchDog : %d", WATCHDOG);
}
robot->unlock(); robot->unlock();
} }
@ -353,6 +367,7 @@ RosAriaNode::~RosAriaNode()
int RosAriaNode::Setup() int RosAriaNode::Setup()
{ {
WATCHDOG=1;
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates. // called once per instance, and these objects need to persist until the process terminates.
@ -666,6 +681,12 @@ void RosAriaNode::publish()
sonar_pub.publish(cloud); sonar_pub.publish(cloud);
} }
} // end if sonar_enabled } // end if sonar_enabled
if(watchdog.toSec()+WATCHDOG<ros::Time::now().toSec())
{
robot->comInt(ArCommands::ESTOP,0);
// ROS_INFO("WATCHDOG %d", WATCHDOG);
}
} }
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
@ -724,6 +745,7 @@ void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{ {
veltime = ros::Time::now(); veltime = ros::Time::now();
watchdog= ros::Time::now();
if(stop_motors_msg.data) if(stop_motors_msg.data)
{ {
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() ); ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );