diff --git a/RosAria.cpp b/RosAria.cpp
index 891919885f5fdf691db303583a537832d77f7434..3e96027fb91b8ab4cd8275290c790d8150497050 100644
--- a/RosAria.cpp
+++ b/RosAria.cpp
@@ -10,6 +10,8 @@
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <sensor_msgs/PointCloud.h> //for sonar data
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
#include "nav_msgs/Odometry.h"
#include "rosaria/BumperState.h"
#include "tf/tf.h"
@@ -34,7 +36,7 @@
class RosAriaNode
{
public:
- RosAriaNode();
+ RosAriaNode(ros::NodeHandle n);
virtual ~RosAriaNode();
public:
@@ -50,10 +52,10 @@ class RosAriaNode
protected:
ros::NodeHandle n;
- ros::NodeHandle private_nh;
ros::Publisher pose_pub;
ros::Publisher bumpers_pub;
ros::Publisher sonar_pub;
+ ros::Publisher sonar_pointcloud2_pub;
ros::Publisher voltage_pub;
ros::Publisher recharge_state_pub;
@@ -95,8 +97,12 @@ class RosAriaNode
std::string frame_id_bumper;
std::string frame_id_sonar;
- //Sonar support
- bool use_sonar; // enable and publish sonars
+ // flag indicating whether sonar was enabled or disabled on the robot
+ bool sonar_enabled;
+
+ // enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects.
+ bool publish_sonar;
+ bool publish_sonar_pointcloud2;
// Debug Aria
bool debug_aria;
@@ -113,43 +119,44 @@ void RosAriaNode::readParameters()
{
// Robot Parameters
robot->lock();
- if (private_nh.hasParam("TicksMM"))
+ ros::NodeHandle n_("~");
+ if (n_.hasParam("TicksMM"))
{
- private_nh.getParam( "TicksMM", TicksMM);
+ n_.getParam( "TicksMM", TicksMM);
ROS_INFO("Setting TicksMM from ROS Parameter: %d", TicksMM);
robot->comInt(93, TicksMM);
}
else
{
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
- private_nh.setParam( "TicksMM", TicksMM);
- ROS_INFO("Setting TicksMM from robot EEPROM: %d", TicksMM);
+ n_.setParam( "TicksMM", TicksMM);
+ ROS_INFO("Setting TicksMM from robot controller stored configuration: %d", TicksMM);
}
- if (private_nh.hasParam("DriftFactor"))
+ if (n_.hasParam("DriftFactor"))
{
- private_nh.getParam( "DriftFactor", DriftFactor);
+ n_.getParam( "DriftFactor", DriftFactor);
ROS_INFO("Setting DriftFactor from ROS Parameter: %d", DriftFactor);
robot->comInt(89, DriftFactor);
}
else
{
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
- private_nh.setParam( "DriftFactor", DriftFactor);
- ROS_INFO("Setting DriftFactor from robot EEPROM: %d", DriftFactor);
+ n_.setParam( "DriftFactor", DriftFactor);
+ ROS_INFO("Setting DriftFactor from robot controller stored configuration: %d", DriftFactor);
}
- if (private_nh.hasParam("RevCount"))
+ if (n_.hasParam("RevCount"))
{
- private_nh.getParam( "RevCount", RevCount);
+ n_.getParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from ROS Parameter: %d", RevCount);
robot->comInt(88, RevCount);
}
else
{
RevCount = robot->getOrigRobotConfig()->getRevCount();
- private_nh.setParam( "RevCount", RevCount);
- ROS_INFO("Setting RevCount from robot EEPROM: %d", RevCount);
+ n_.setParam( "RevCount", RevCount);
+ ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount);
}
robot->unlock();
}
@@ -198,12 +205,6 @@ void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t
ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
robot->setTransDecel(value);
}
- value = config.trans_vel_max * 1000;
- if(value != robot->getTransVelMax() and value > 0)
- {
- ROS_INFO("Setting TransVelMax from Dynamic Reconfigure: %d", value);
- robot->setTransVelMax(value);
- }
value = config.lat_accel * 1000;
if(value != robot->getLatAccel() and value > 0)
@@ -239,35 +240,40 @@ void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t
void RosAriaNode::sonarConnectCb()
{
+ publish_sonar = (sonar_pub.getNumSubscribers() > 0);
+ publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
robot->lock();
- if (sonar_pub.getNumSubscribers() == 0)
+ if (publish_sonar || publish_sonar_pointcloud2)
{
- robot->disableSonar();
- use_sonar = false;
+ robot->enableSonar();
+ sonar_enabled = false;
}
- else
+ else if(!publish_sonar && !publish_sonar_pointcloud2)
{
- robot->enableSonar();
- use_sonar = true;
+ robot->disableSonar();
+ sonar_enabled = true;
}
robot->unlock();
}
-RosAriaNode::RosAriaNode() :
- myPublishCB(this, &RosAriaNode::publish),private_nh("~"), n(), serial_port(""), serial_baud(0), use_sonar(false)
+RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
+ myPublishCB(this, &RosAriaNode::publish), serial_port(""), serial_baud(0),
+ sonar_enabled(false), publish_sonar(false), publish_sonar_pointcloud2(false)
{
+ // read in runtime parameters
+ n = nh;
- // !!! port !!!
- private_nh.param( "port", serial_port, std::string("/dev/ttyUSB0") );
+ // port and baud
+ n.param( "port", serial_port, std::string("/dev/ttyUSB0") );
ROS_INFO( "RosAria: using port: [%s]", serial_port.c_str() );
- private_nh.param("baud", serial_baud, 0);
+ n.param("baud", serial_baud, 0);
if(serial_baud != 0)
ROS_INFO("RosAria: using serial port baud rate %d", serial_baud);
// handle debugging more elegantly
- private_nh.param( "debug_aria", debug_aria, false ); // default not to debug
- private_nh.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
+ n.param( "debug_aria", debug_aria, false ); // default not to debug
+ n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
// Figure out what frame_id's to use. if a tf_prefix param is specified,
// it will be added to the beginning of the frame_ids.
@@ -277,7 +283,7 @@ RosAriaNode::RosAriaNode() :
// will result in the frame_ids being set to /MyRobot/odom etc,
// rather than /odom. This is useful for Multi Robot Systems.
// See ROS Wiki for further details.
- tf_prefix = tf::getPrefixParam(private_nh);
+ tf_prefix = tf::getPrefixParam(n);
frame_id_odom = tf::resolve(tf_prefix, "odom");
frame_id_base_link = tf::resolve(tf_prefix, "base_link");
frame_id_bumper = tf::resolve(tf_prefix, "bumpers_frame");
@@ -290,9 +296,12 @@ RosAriaNode::RosAriaNode() :
// See ros::NodeHandle API docs.
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
- sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
- boost::bind(&RosAriaNode::sonarConnectCb, this),
- boost::bind(&RosAriaNode::sonarConnectCb, this));
+ sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
+ boost::bind(&RosAriaNode::sonarConnectCb, this),
+ boost::bind(&RosAriaNode::sonarConnectCb, this));
+ sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50,
+ boost::bind(&RosAriaNode::sonarConnectCb, this),
+ boost::bind(&RosAriaNode::sonarConnectCb, this));
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
@@ -570,16 +579,17 @@ void RosAriaNode::publish()
}
// Publish sonar information, if enabled.
- if (use_sonar) {
+ if (publish_sonar || publish_sonar_pointcloud2)
+ {
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
-
+
- // Log debugging info
- std::stringstream sonar_debug_info;
+ std::stringstream sonar_debug_info; // Log debugging info
sonar_debug_info << "Sonar readings: ";
+
for (int i = 0; i < robot->getNumSonar(); i++) {
ArSensorReading* reading = NULL;
reading = robot->getSonarReading(i);
@@ -587,7 +597,7 @@ void RosAriaNode::publish()
ROS_WARN("RosAria: Did not receive a sonar reading.");
continue;
}
-
+
// getRange() will return an integer between 0 and 5000 (5m)
sonar_debug_info << reading->getRange() << " ";
@@ -601,7 +611,7 @@ void RosAriaNode::publish()
// << ", " << reading->getLocalY()
// << ") from (" << sensor.getX() << ", "
// << sensor.getY() << ") ;; " ;
-
+
//add sonar readings (robot-local coordinate frame) to cloud
geometry_msgs::Point32 p;
p.x = reading->getLocalX() / 1000.0;
@@ -611,9 +621,26 @@ void RosAriaNode::publish()
}
ROS_DEBUG_STREAM(sonar_debug_info.str());
- sonar_pub.publish(cloud);
- }
+ // publish topic(s)
+
+ if(publish_sonar_pointcloud2)
+ {
+ sensor_msgs::PointCloud2 cloud2;
+ if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
+ {
+ ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
+ }
+ else
+ {
+ sonar_pointcloud2_pub.publish(cloud2);
+ }
+ }
+ if(publish_sonar)
+ {
+ sonar_pub.publish(cloud);
+ }
+ } // end if sonar_enabled
}
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
@@ -658,9 +685,10 @@ RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
int main( int argc, char** argv )
{
ros::init(argc,argv, "RosAria");
+ ros::NodeHandle n(std::string("~"));
Aria::init();
- RosAriaNode *node = new RosAriaNode;
+ RosAriaNode *node = new RosAriaNode(n);
if( node->Setup() != 0 )
{
diff --git a/cfg/RosAria.cfg b/cfg/RosAria.cfg
index fb9c4cf4a25080a831c7752073a49cc867ec9d7c..309e3322cfd634b81ffe51f6927506614877a6de 100755
--- a/cfg/RosAria.cfg
+++ b/cfg/RosAria.cfg
@@ -8,7 +8,6 @@ gen = ParameterGenerator()
gen.add("trans_accel", double_t, 0, "Translational acceleration (m/s^2)")
gen.add("trans_decel", double_t, 0, "Translational deceleration (m/s^2)" )
-gen.add("trans_vel_max", double_t, 0, "Translational velocity" )
gen.add("lat_accel" , double_t, 0, "Lateral acceleration (m/s^2)" )
gen.add("lat_decel" , double_t, 0, "Lateral deceleration (m/s^2)" )
gen.add("rot_accel" , double_t, 0, "Rotational acceleration (rad/s^2)" )