Skip to content

ros interface for ModelightIndicators on stage and some fixes #64

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.

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

Open
wants to merge 5 commits into
base: lunar-devel
Choose a base branch
from
Open
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -14,7 +14,7 @@ find_package(catkin REQUIRED

find_package(Boost REQUIRED COMPONENTS system thread)

find_package(stage REQUIRED)
find_package(stage REQUIRED PATHS /home/scifiswapnil/stage-buildqcc/)

include_directories(
${catkin_INCLUDE_DIRS}
95 changes: 70 additions & 25 deletions src/stageros.cpp
Original file line number Diff line number Diff line change
@@ -48,6 +48,8 @@
#include <sensor_msgs/CameraInfo.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>
#include <rosgraph_msgs/Clock.h>

#include <std_srvs/Empty.h>
@@ -62,6 +64,8 @@
#define BASE_SCAN "base_scan"
#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
#define CMD_VEL "cmd_vel"
#define LIGHTS "light"
#define SETLIGHTS "set_light"

// Our node
class StageNode
@@ -78,6 +82,7 @@ class StageNode
std::vector<Stg::ModelCamera *> cameramodels;
std::vector<Stg::ModelRanger *> lasermodels;
std::vector<Stg::ModelPosition *> positionmodels;
std::vector<Stg::ModelLightIndicator *> lightmodels;

//a structure representing a robot inthe simulator
struct StageRobot
@@ -86,6 +91,7 @@ class StageNode
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::ModelLightIndicator *> lightmodels; //multiple lights per position

//ros publishers
ros::Publisher odom_pub; //one odom
@@ -94,8 +100,9 @@ class StageNode
std::vector<ros::Publisher> image_pubs; //multiple images
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> laser_pubs; //multiple light

std::vector<ros::Subscriber> light_subs; //multiple lasers
ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
};

@@ -144,7 +151,7 @@ class StageNode
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names);
StageNode(int argc, char** argv, const char* fname);
~StageNode();

// Subscribe to models of interest. Currently, we find and subscribe
@@ -161,6 +168,8 @@ class StageNode

// Message callback for a MsgBaseVel message, which set velocities.
void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);
void lightReceived(int idx, const boost::shared_ptr<std_msgs::Int8 const>& msg);
void lightColorReceived(int idx, const boost::shared_ptr<std_msgs::String const>& msg);

// Service callback for soft reset
bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
@@ -183,11 +192,11 @@ StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const

if ((found==std::string::npos) && umn)
{
snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
snprintf(buf, sizeof(buf), "%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
}
else
{
snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
snprintf(buf, sizeof(buf), "robot_%u/%s", (unsigned int)robotID, name);
}

return buf;
@@ -209,19 +218,19 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model

if ((found==std::string::npos) && umn)
{
snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
snprintf(buf, sizeof(buf), "%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
}
else
{
snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
snprintf(buf, sizeof(buf), "robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
}

return buf;
}
else
{
static char buf[100];
snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
snprintf(buf, sizeof(buf), "%s_%u", name, (unsigned int)deviceID);
return buf;
}
}
@@ -234,6 +243,11 @@ StageNode::ghfunc(Stg::Model* mod, StageNode* node)
if (dynamic_cast<Stg::ModelRanger *>(mod)) {
node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
}

if (dynamic_cast<Stg::ModelLightIndicator *>(mod)) {
node->lightmodels.push_back(dynamic_cast<Stg::ModelLightIndicator *>(mod));
}

if (dynamic_cast<Stg::ModelPosition *>(mod)) {
Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
// remember initial poses
@@ -260,6 +274,21 @@ StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Resp
}


void
StageNode::lightReceived(int idx, const boost::shared_ptr<std_msgs::Int8 const>& msg)
{
boost::mutex::scoped_lock lock(msg_lock);
this->lightmodels[idx]->SetState(msg->data) ;
}

void
StageNode::lightColorReceived(int idx, const boost::shared_ptr<std_msgs::String const>& msg)
{
boost::mutex::scoped_lock lock(msg_lock);
this->lightmodels[idx]->SetLightColor(msg->data) ;
}



void
StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg)
@@ -271,20 +300,29 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist
this->base_last_cmd = this->sim_time;
}

StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
StageNode::StageNode(int argc, char** argv, const char* fname)
{
this->use_model_names = use_model_names;
bool gui;
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
double t;
ros::NodeHandle localn("~");

if (!localn.getParam("gui", gui))
gui = true;

if (!localn.getParam("use_model_names", use_model_names))
use_model_names = false;

if(!localn.getParam("base_watchdog_timeout", t))
t = 0.2;
this->base_watchdog_timeout.fromSec(t);

if(!localn.getParam("is_depth_canonical", isDepthCanonical))
isDepthCanonical = true;

this->use_model_names = use_model_names;
this->base_watchdog_timeout.fromSec(t);

// We'll check the existence of the world file, because libstage doesn't
// expose its failure to open it. Could go further with checks (e.g., is
// it readable by this user).
@@ -354,16 +392,34 @@ StageNode::SubscribeModels()
}
}

for (size_t s = 0; s < this->lightmodels.size(); s++)
{
if (this->lightmodels[s] and this->lightmodels[s]->Parent() == new_robot->positionmodel)
{
new_robot->lightmodels.push_back(this->lightmodels[s]);
this->lightmodels[s]->Subscribe();

ROS_INFO( "subscribed to Stage light model \"%s\"", this->lightmodels[s]->Token() );
}
}

// TODO - print the topic names nicely as well
ROS_INFO("Robot %s provided %lu rangers and %lu cameras",
ROS_INFO("Robot %s provided %lu rangers %lu lights and %lu cameras",
new_robot->positionmodel->Token(),
new_robot->lasermodels.size(),
new_robot->lightmodels.size(),
new_robot->cameramodels.size() );

new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
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));

for (size_t s = 0; s < new_robot->lightmodels.size(); ++s)
{
new_robot->light_subs.push_back(n_.subscribe<std_msgs::Int8>(mapName(LIGHTS, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::lightReceived, this, r, _1)));
new_robot->light_subs.push_back(n_.subscribe<std_msgs::String>(mapName(SETLIGHTS, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::lightColorReceived, this, r, _1)));
}

for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
{
if (new_robot->lasermodels.size() == 1)
@@ -410,7 +466,7 @@ StageNode::UpdateWorld()
{
return this->world->UpdateAll();
}

bool global_state = true;
void
StageNode::WorldCallback()
{
@@ -443,7 +499,6 @@ StageNode::WorldCallback()
for (size_t r = 0; r < this->robotmodels_.size(); ++r)
{
StageRobot const * robotmodel = this->robotmodels_[r];

//loop on the laser devices for the current robot
for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
{
@@ -757,17 +812,7 @@ main(int argc, char** argv)

ros::init(argc, argv, "stageros");

bool gui = true;
bool use_model_names = false;
for(int i=0;i<(argc-1);i++)
{
if(!strcmp(argv[i], "-g"))
gui = false;
if(!strcmp(argv[i], "-u"))
use_model_names = true;
}

StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
StageNode sn(argc-1,argv,argv[argc-1]);

if(sn.SubscribeModels() != 0)
exit(-1);
8 changes: 8 additions & 0 deletions world/willow-four-erratics.world
Original file line number Diff line number Diff line change
@@ -4,6 +4,13 @@ define block model
gui_nose 0
)

define light lightindicator
(
# generic model properties
size [0.03 0.03 0.03]
color "red"
)

define topurg ranger
(
sensor(
@@ -24,6 +31,7 @@ define erratic position
origin [-0.05 0 0 0]
gui_nose 1
drive "diff"
light()
topurg(pose [ 0.050 0.000 0 0.000 ])
)