i am trying to load a light using light message but it is not working. Here is the code
namespace gazebo { class WorldPluginTutorial : public WorldPlugin { public: WorldPluginTutorial() : WorldPlugin() { printf("Hello World!\n"); }
public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
printf("Hello World Load!\n");
this->model = _world;
printf("%s\n", (this->model->GetName()).c_str());
// Initialize ros, if it has not already bee initialized.
if (!ros::isInitialized())
{
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "gazebo_client",
ros::init_options::NoSigintHandler);
}
// Create our ROS node. This acts in a similar manner to
// the Gazebo node
this->rosNode.reset(new ros::NodeHandle("gazebo_client"));
// Create a named topic, and subscribe to it.
ros::SubscribeOptions so =
ros::SubscribeOptions::create<std_msgs::Float32>(
"/" + this->model->GetName() + "/vel_cmd",
1,
boost::bind(&WorldPluginTutorial::OnRosMsg, this, _1),
ros::VoidPtr(), &this->rosQueue);
this->rosSub = this->rosNode->subscribe(so);
// Spin up the queue helper thread.
this->rosQueueThread =
std::thread(std::bind(&WorldPluginTutorial::QueueThread, this));
}
public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
{
//this->SetVelocity(_msg->data);
printf("Entered WorldPluginTutorial::OnRosMsg\n");
//w->InsertModelFile("model://sun");
//physics::WorldPtr w = this->model->GetWorld();
//unsigned int i = this->model->LightCount();
sdf::SDF point;
point.SetFromString("<?xml version='1.0' ?>\
<sdf version='1.5'>\
<!-- Light Source -->\
<light name='point_light' type='point'>\
<pose frame=''>2 -2 1 0 -0 0</pose>\
<diffuse>0.5 0.5 0.5 1</diffuse>\
<specular>0.1 0.1 0.1 1</specular>\
<attenuation>\
<range>20</range>\
<constant>0.5</constant>\
<linear>0.01</linear>\
<quadratic>0.001</quadratic>\
</attenuation>\
<cast_shadows>0</cast_shadows>\
<direction>0 0 -1</direction>\
</light>\
</sdf>");
sdf::ElementPtr light = point.Root()->GetElement("light");
msgs::Light msg = gazebo::msgs::LightFromSDF(light);
//this->model->LoadLigt(light);
transport::NodePtr node(new transport::Node());
node->Init(this->model->GetName());
transport::PublisherPtr lightPub
= node->Advertise<msgs::Light>("~/light");
lightPub->Publish(msg);
unsigned int i = this->model->Lights().size();
printf("Light count = %d\n", i);
//w->InsertModelFile("model://sun");
//w->InsertModelFile("model://sun");
}
/// \brief ROS helper function that processes messages
private: void QueueThread()
{
static const double timeout = 0.01;
while (this->rosNode->ok())
{
this->rosQueue.callAvailable(ros::WallDuration(timeout));
}
}
private: physics::WorldPtr model;
private: std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief A ROS subscriber
private: ros::Subscriber rosSub;
/// \brief A ROS callbackqueue that helps process messages
private: ros::CallbackQueue rosQueue;
/// \brief A thread the keeps running the rosQueue
private: std::thread rosQueueThread;
}; GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial) }
Originally posted by Ash_100 on Gazebo Answers with karma: 9 on 2017-10-05
Post score: 0
