-
Notifications
You must be signed in to change notification settings - Fork 8
/
ros_plugin.cc
67 lines (52 loc) · 2.21 KB
/
ros_plugin.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
#ifndef ROS_PLUGIN_CC
#define ROS_PLUGIN_CC
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_config.h>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/Joint.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/Node.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
namespace gazebo {
class ROSPlugin : public SystemPlugin {
public: ROSPlugin() {}
~ROSPlugin() {}
public: void Load(int argc, char **argv)
{
std::cout << "---------------------------------------" << std::endl;
std::cout << "douniwan" << std::endl;
std::cout << "argc = " << argc << ", " << argv[0] << std::endl;
std::cout << "---------------------------------------" << std::endl;
if (!ros::isInitialized()) {
ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler);
}
this->mRosNode.reset(new ros::NodeHandle("~"));
this->mRosClockPub = this->mRosNode->advertise<rosgraph_msgs::Clock>("/clock", 10);
mWorldCreatedConnection = gazebo::event::Events::ConnectWorldCreated(boost::bind(&ROSPlugin::OnWorldCreated, this, _1));
mUpdateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ROSPlugin::OnWorldUpdateBegin, this));
}
private: event::ConnectionPtr mUpdateConnection;
private: event::ConnectionPtr mWorldCreatedConnection;
private: void OnWorldCreated(std::string world_name)
{
std::cout << "world [" << world_name << "] created" << std::endl;
mWorld = gazebo::physics::get_world(world_name);
}
private: std::unique_ptr<ros::NodeHandle> mRosNode;
private: ros::Publisher mRosClockPub;
private: void OnWorldUpdateBegin()
{
common::Time curTime = mWorld->SimTime();
rosgraph_msgs::Clock rosTime;
rosTime.clock.fromSec(curTime.Double());
mRosClockPub.publish(rosTime);
}
private: gazebo::physics::WorldPtr mWorld;
};
GZ_REGISTER_SYSTEM_PLUGIN(ROSPlugin)
};
#endif