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

Added function to publish robot_info. Also adjusted stop handling

Browse Source
This commit is contained in:
Pioneer3 2018-09-28 16:51:35 +02:00
parent 8ae1f486c8
commit 3efc57bcf5
1 changed files with 40 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
RosAria.cpp
View File

@ -34,7 +34,7 @@
#include <sstream> #include <sstream>
#include <cstdlib>
// Node that interfaces between ROS and mobile robot base features via ARIA library. // Node that interfaces between ROS and mobile robot base features via ARIA library.
// //
@ -60,6 +60,7 @@ public:
//void cmd_disable_motors_cb(); //void cmd_disable_motors_cb();
void spin(); void spin();
void publish(); void publish();
void publish_robot_info();
void sonarConnectCb(); void sonarConnectCb();
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level); void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
void readParameters(); void readParameters();
@ -85,14 +86,19 @@ protected:
ros::Subscriber cmdvel_sub; ros::Subscriber cmdvel_sub;
// Olek new subscribers // Olek new subscribers and publishers
ros::Subscriber restrictions_sub; ros::Subscriber restrictions_sub;
ros::Subscriber userstop_sub; ros::Subscriber userstop_sub;
ros::Subscriber masterstop_sub; ros::Subscriber masterstop_sub;
ros::Subscriber clutch_sub; ros::Subscriber clutch_sub;
ros::Publisher robot_info_pub;
// true means robot is running, false means robot is stopped
bool robot_state;
bool userstop_state; bool userstop_state;
bool masterstop_state; bool masterstop_state;
bool clutch_state; bool clutch_state;
ros::ServiceServer enable_srv; ros::ServiceServer enable_srv;
@ -390,6 +396,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
masterstop_sub = n.subscribe( "/PIONIER/master_stop", 1, &RosAriaNode::masterstop_cb,this); masterstop_sub = n.subscribe( "/PIONIER/master_stop", 1, &RosAriaNode::masterstop_cb,this);
clutch_sub = n.subscribe( "clutch", 1, &RosAriaNode::clutch_cb,this); clutch_sub = n.subscribe( "clutch", 1, &RosAriaNode::clutch_cb,this);
robot_info_pub = n.advertise<rosaria_msgs::RobotInfoMsg>("robot_info", 1, true /*latch*/ );
// advertise enable/disable services // advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this); enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
@ -574,6 +581,23 @@ void RosAriaNode::spin()
ros::spin(); ros::spin();
} }
void RosAriaNode::publish_robot_info()
{
rosaria_msgs::RobotInfoMsg robot_info_msg;
robot_info_msg.robot_id.data = atoi(std::getenv("PIONIER_ID"));
robot_info_msg.battery_voltage.data = robot->getRealBatteryVoltageNow();
robot_info_msg.twist.linear.x = robot->getVel()/1000;
robot_info_msg.twist.linear.y = robot->getLatVel()/1000.0;
robot_info_msg.twist.angular.z = robot->getRotVel()*M_PI/180;
robot_info_msg.state.data = robot_state;
robot_info_msg.clutch.data = clutch_state;
// robot_info_msg.header.stamp = ros::Time::now();
robot_info_pub.publish(robot_info_msg);
}
void RosAriaNode::publish() void RosAriaNode::publish()
{ {
robot->requestEncoderPackets(); robot->requestEncoderPackets();
@ -927,6 +951,12 @@ void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
{ {
// User stop released // User stop released
userstop_state = true; userstop_state = true;
if (masterstop_state)
{
// If master stop is not pressed we can set robot in running state
robot_state = true;
}
} }
else else
{ {
@ -941,6 +971,7 @@ void RosAriaNode::userstop_cb(const std_msgs::Bool &msg)
robot->comInt(ArCommands::ESTOP,0); // ? robot->comInt(ArCommands::ESTOP,0); // ?
robot->unlock(); robot->unlock();
robot_state = false;
userstop_state = false; userstop_state = false;
} }
} }
@ -951,6 +982,12 @@ void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
{ {
// Master stop released // Master stop released
masterstop_state = true; masterstop_state = true;
if (userstop_state)
{
// If user stop is not pressed we can set robot in running state
robot_state = true;
}
} }
else else
{ {
@ -965,6 +1002,7 @@ void RosAriaNode::masterstop_cb(const std_msgs::Bool &msg)
robot->comInt(ArCommands::ESTOP,0); // ? robot->comInt(ArCommands::ESTOP,0); // ?
robot->unlock(); robot->unlock();
robot_state = false;
masterstop_state = false; masterstop_state = false;
} }
} }