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 <math.h>
#ifdef ADEPT_PKG
@ -52,6 +53,7 @@ class RosAriaNode
void readParameters();
protected:
int WATCHDOG;
ros::NodeHandle n;
ros::Publisher pose_pub;
ros::Publisher bumpers_pub;
@ -124,6 +126,8 @@ class RosAriaNode
//Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
bool stop_robot;
std_msgs::Bool stop_motors_msg;
ros::Time watchdog;
};
void RosAriaNode::readParameters()
@ -169,6 +173,16 @@ void RosAriaNode::readParameters()
n_.setParam( "RevCount", 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();
}
@ -353,6 +367,7 @@ RosAriaNode::~RosAriaNode()
int RosAriaNode::Setup()
{
WATCHDOG=1;
// 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.
@ -666,6 +681,12 @@ void RosAriaNode::publish()
sonar_pub.publish(cloud);
}
} // 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)
@ -724,6 +745,7 @@ void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
veltime = ros::Time::now();
watchdog= ros::Time::now();
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() );