diff --git a/CMakeLists.txt b/CMakeLists.txt index d6bc35e..1009cfa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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} diff --git a/src/stageros.cpp b/src/stageros.cpp index 39d040a..4f49a6a 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -48,6 +48,8 @@ #include #include #include +#include +#include #include #include @@ -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 cameramodels; std::vector lasermodels; std::vector positionmodels; + std::vector lightmodels; //a structure representing a robot inthe simulator struct StageRobot @@ -86,6 +91,7 @@ class StageNode Stg::ModelPosition* positionmodel; //one position std::vector cameramodels; //multiple cameras per position std::vector lasermodels; //multiple rangers per position + std::vector lightmodels; //multiple lights per position //ros publishers ros::Publisher odom_pub; //one odom @@ -94,8 +100,9 @@ class StageNode std::vector image_pubs; //multiple images std::vector depth_pubs; //multiple depths std::vector camera_pubs; //multiple cameras - std::vector laser_pubs; //multiple lasers - + std::vector laser_pubs; //multiple light + + std::vector 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& msg); + void lightReceived(int idx, const boost::shared_ptr& msg); + void lightColorReceived(int idx, const boost::shared_ptr& 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,11 +218,11 @@ 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; @@ -221,7 +230,7 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model 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(mod)) { node->lasermodels.push_back(dynamic_cast(mod)); } + + if (dynamic_cast(mod)) { + node->lightmodels.push_back(dynamic_cast(mod)); + } + if (dynamic_cast(mod)) { Stg::ModelPosition * p = dynamic_cast(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& msg) +{ + boost::mutex::scoped_lock lock(msg_lock); + this->lightmodels[idx]->SetState(msg->data) ; +} + +void +StageNode::lightColorReceived(int idx, const boost::shared_ptr& msg) +{ + boost::mutex::scoped_lock lock(msg_lock); + this->lightmodels[idx]->SetLightColor(msg->data) ; +} + + void StageNode::cmdvelReceived(int idx, const boost::shared_ptr& msg) @@ -271,20 +300,29 @@ StageNode::cmdvelReceived(int idx, const boost::shared_ptrbase_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(mapName(ODOM, r, static_cast(new_robot->positionmodel)), 10); new_robot->ground_truth_pub = n_.advertise(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast(new_robot->positionmodel)), 10); new_robot->cmdvel_sub = n_.subscribe(mapName(CMD_VEL, r, static_cast(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(mapName(LIGHTS, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::lightReceived, this, r, _1))); + new_robot->light_subs.push_back(n_.subscribe(mapName(SETLIGHTS, r, static_cast(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); diff --git a/world/willow-four-erratics.world b/world/willow-four-erratics.world index bb9405c..5ad462d 100644 --- a/world/willow-four-erratics.world +++ b/world/willow-four-erratics.world @@ -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 ]) )