Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Implement use of sonar msg #50

Open
wants to merge 1 commit into
base: lunar-devel
Choose a base branch
from
Open

Implement use of sonar msg #50

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
104 changes: 100 additions & 4 deletions src/stageros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
Expand All @@ -60,6 +61,7 @@
#define CAMERA_INFO "camera_info"
#define ODOM "odom"
#define BASE_SCAN "base_scan"
#define SONAR "sonar" // want naming "sonar_x/range, so concatenate "range" in later naming
#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
#define CMD_VEL "cmd_vel"

Expand All @@ -76,16 +78,20 @@ class StageNode

// The models that we're interested in
std::vector<Stg::ModelCamera *> cameramodels;
std::vector<Stg::ModelRanger *> lasermodels;
std::vector<Stg::ModelRanger *> rangermodels;
std::vector<Stg::ModelPosition *> positionmodels;
std::vector<Stg::ModelRanger *> lasermodels;
std::vector<Stg::ModelRanger *> sonarmodels;

//a structure representing a robot inthe simulator
struct StageRobot
{
//stage related models
Stg::ModelPosition* positionmodel; //one position
std::vector<Stg::ModelCamera *> cameramodels; //multiple cameras per position
std::vector<Stg::ModelRanger *> lasermodels; //multiple rangers per position
std::vector<Stg::ModelRanger *> rangermodels; //multiple rangers per position, contains lasers and sonars
std::vector<Stg::ModelRanger *> lasermodels; // multiple lasers per position
std::vector<Stg::ModelRanger *> sonarmodels; // multiple sonars per position

//ros publishers
ros::Publisher odom_pub; //one odom
Expand All @@ -95,6 +101,7 @@ class StageNode
std::vector<ros::Publisher> depth_pubs; //multiple depths
std::vector<ros::Publisher> camera_pubs; //multiple cameras
std::vector<ros::Publisher> laser_pubs; //multiple lasers
std::vector<ros::Publisher> sonar_pubs; // multiple sonars

ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
};
Expand Down Expand Up @@ -364,6 +371,44 @@ StageNode::SubscribeModels()
new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));

// break rangermodes -> lasermdoels + sonarmodels
int num_lasers = 0;
int num_sonars = 0;
for (size_t s = 0; s < new_robot->rangermodels.size(); s++)
{
// assumed only one sensor per ranger, as Stage currently only supports this
// sonar is sample_count == 1
if (rangermodels[s]->GetSensors()[0].sample_count == 1)
{
new_robot->sonarmodels.push_back(rangermodels[s]);
num_sonars++;
}
else
{
new_robot->lasermodels.push_back(rangermodels[s]);
num_lasers++;
}
}

// create publishers for lasers and sonars with correct labelling
// add number label on each topic with more than one device
if (num_lasers == 1)
new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
else
{
for (size_t s = 0; s < num_lasers; ++s)
new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
}

// add "/range" at end of name to get naming style "sonar_3/range"
if (num_sonars == 1)
new_robot->sonar_pubs.push_back(n_.advertise<sensor_msgs::Range>(std::string(mapName(SONAR, r, static_cast<Stg::Model*>(new_robot->positionmodel))) + "/range", 10));
else
{
for (size_t s = 0; s < num_sonars; ++s)
new_robot->sonar_pubs.push_back(n_.advertise<sensor_msgs::Range>(std::string(mapName(SONAR, r, s, static_cast<Stg::Model*>(new_robot->positionmodel))) + "/range", 10));
}

for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
{
if (new_robot->lasermodels.size() == 1)
Expand Down Expand Up @@ -444,14 +489,14 @@ StageNode::WorldCallback()
{
StageRobot const * robotmodel = this->robotmodels_[r];

//loop on the laser devices for the current robot
// loop on the laser devices for the current robot
for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
{
Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s];
const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->GetSensors();

if( sensors.size() > 1 )
ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
ROS_WARN_ONCE( "ROS Stage currently supports rangers with 1 sensor only." );

// for now we access only the zeroth sensor of the ranger - good
// enough for most laser models that have a single beam origin
Expand Down Expand Up @@ -501,6 +546,57 @@ StageNode::WorldCallback()
mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
}

// loop on the sonar devices for the current robot
for (size_t s = 0; s < robotmodel->sonarmodels.size(); ++s)
{
Stg::ModelRanger const* sonarmodel = robotmodel->sonarmodels[s];
const std::vector<Stg::ModelRanger::Sensor>& sensors = sonarmodel->GetSensors();

if (sensors.size() > 1)
ROS_WARN("ROS Stage currently supports sonars with 1 sensor only.");

// for now we access only the zeroth sensor of the sonar - good
// enough for most sonar models that have a single beam origin
const Stg::ModelRanger::Sensor& sensor = sensors[0];

if (sensor.ranges.size())
{
// Translate into ROS message format and publish
sensor_msgs::Range msg;
msg.field_of_view = sensor.fov;
msg.min_range = sensor.range.min;
msg.max_range = sensor.range.max;
msg.range = sensor.ranges[0];

// assume all are sonar, not infrared. This choice not currently in Stage
msg.radiation_type = sensor_msgs::Range::ULTRASOUND;

if (robotmodel->sonarmodels.size() > 1)
msg.header.frame_id = mapName("base_sonar_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
else
msg.header.frame_id = mapName("base_sonar_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));

msg.header.stamp = sim_time;
robotmodel->sonar_pubs[s].publish(msg);
}

// Also publish the base->sonar_link Tx. This could eventually move
// into being retrieved from the param server as a static Tx.
Stg::Pose lp = sonarmodel->GetPose();
tf::Quaternion sonarQ;
sonarQ.setRPY(0.0, 0.0, lp.a);
tf::Transform txSonar = tf::Transform(sonarQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));

if (robotmodel->sonarmodels.size() > 1)
tf.sendTransform(tf::StampedTransform(txSonar, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_sonar_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
else
tf.sendTransform(tf::StampedTransform(txSonar, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_sonar_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
}

//the position of the robot
tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
sim_time,
Expand Down