From f3b647f731e5885b081504978b5889ceb4309104 Mon Sep 17 00:00:00 2001 From: scifiswapnil Date: Fri, 24 Apr 2020 02:36:39 +0530 Subject: [PATCH 1/5] update : parameterised gui and use_model_names --- src/stageros.cpp | 29 ++++++++++++++--------------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 39d040a1..800679e0 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -144,7 +144,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 @@ -271,20 +271,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). @@ -757,17 +766,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); From eb6c8594a42e2b2a3c1243c5438951df0c86dc38 Mon Sep 17 00:00:00 2001 From: scifiswapnil Date: Fri, 24 Apr 2020 20:06:46 +0530 Subject: [PATCH 2/5] update : ros topic interface for ModelLightIndicators --- src/stageros.cpp | 43 ++++++++++++++++++++++++++++++++++++++----- 1 file changed, 38 insertions(+), 5 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 800679e0..527aeaae 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -62,6 +63,7 @@ #define BASE_SCAN "base_scan" #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth" #define CMD_VEL "cmd_vel" +#define LIGHTS "light" // Our node class StageNode @@ -78,6 +80,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 +89,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 +98,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 }; @@ -161,6 +166,7 @@ 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); // Service callback for soft reset bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); @@ -234,6 +240,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 +271,12 @@ 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::cmdvelReceived(int idx, const boost::shared_ptr& msg) @@ -363,16 +380,33 @@ 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->lasermodels.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))); + } + for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) { if (new_robot->lasermodels.size() == 1) @@ -419,7 +453,7 @@ StageNode::UpdateWorld() { return this->world->UpdateAll(); } - +bool global_state = true; void StageNode::WorldCallback() { @@ -452,7 +486,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) { From 1d49387d435a75e4a406beeb8c6420945beaf5a7 Mon Sep 17 00:00:00 2001 From: scifiswapnil Date: Mon, 27 Apr 2020 23:51:29 +0530 Subject: [PATCH 3/5] fix : remove '/' for melodic frame names as tf pub throws error --- src/stageros.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 527aeaae..ce08257e 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -40,6 +40,7 @@ // roscpp #include +// #include #include #include #include @@ -189,11 +190,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; @@ -215,11 +216,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; @@ -227,7 +228,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; } } From 79ebf7cab9e47a7436fe355632bbc745b8cc77a6 Mon Sep 17 00:00:00 2001 From: scifiswapnil Date: Fri, 1 May 2020 19:55:16 +0530 Subject: [PATCH 4/5] update : lightIndicator ros interface via topic (msg type : Int8) --- src/stageros.cpp | 9 ++++----- world/willow-four-erratics.world | 8 ++++++++ 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index ce08257e..eddb34a2 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -40,7 +40,6 @@ // roscpp #include -// #include #include #include #include @@ -49,7 +48,7 @@ #include #include #include -#include +#include #include #include @@ -167,7 +166,7 @@ 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 lightReceived(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); @@ -273,7 +272,7 @@ StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Resp void -StageNode::lightReceived(int idx, const boost::shared_ptr& msg) +StageNode::lightReceived(int idx, const boost::shared_ptr& msg) { boost::mutex::scoped_lock lock(msg_lock); this->lightmodels[idx]->SetState(msg->data) ; @@ -405,7 +404,7 @@ StageNode::SubscribeModels() for (size_t s = 0; s < new_robot->lasermodels.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(LIGHTS, r, static_cast(new_robot->positionmodel)), 10, boost::bind(&StageNode::lightReceived, this, r, _1))); } for (size_t s = 0; s < new_robot->lasermodels.size(); ++s) diff --git a/world/willow-four-erratics.world b/world/willow-four-erratics.world index bb9405c5..5ad462d0 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 ]) ) From 44a78b743707d491e5ef3c3d1993c278531b9753 Mon Sep 17 00:00:00 2001 From: scifiswapnil Date: Sun, 3 May 2020 23:12:55 +0530 Subject: [PATCH 5/5] update : led set color ROS interface topic --- CMakeLists.txt | 2 +- src/stageros.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d6bc35ef..1009cfa6 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 eddb34a2..4f49a6aa 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -64,6 +65,7 @@ #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 @@ -167,6 +169,7 @@ 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); @@ -278,6 +281,15 @@ StageNode::lightReceived(int idx, const boost::shared_ptr& 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) { @@ -402,9 +414,10 @@ StageNode::SubscribeModels() 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->lasermodels.size(); ++s) + 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)