17 #include <gazebo/physics/World.hh> 18 #include <gazebo/physics/Model.hh> 50 HarnessPlugin::Load(_parent, _sdf);
53 if (_sdf->HasElement(
"robotNamespace"))
60 <<
"properly initialized. Try starting gazebo with ros plugin:\n" 61 <<
" gazebo -s libgazebo_ros_api_plugin.so\n");
68 "/" + _parent->GetName() +
"/harness/velocity", 1,
73 so = ros::SubscribeOptions::create<std_msgs::Bool>(
74 "/" + _parent->GetName() +
"/harness/detach", 1,
88 this->SetWinchVelocity(msg->data);
102 static const double timeout = 0.01;
ros::Subscriber detachSub_
Subscriber to detach control messages.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
ros::NodeHandle * rosnode_
pointer to ros node
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
std::string robotNamespace_
for setting ROS name space
virtual void OnVelocity(const std_msgs::Float32::ConstPtr &msg)
Receive winch velocity control messages.
#define ROS_FATAL_STREAM_NAMED(name, args)
ros::Subscriber velocitySub_
Subscriber to velocity control messages.
virtual ~GazeboRosHarness()
Destructor.
boost::thread callbackQueueThread_
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
GazeboRosHarness()
Constructor.
boost::shared_ptr< void > VoidPtr
void QueueThread()
Custom callback queue thread.
ros::CallbackQueue queue_
virtual void OnDetach(const std_msgs::Bool::ConstPtr &msg)
Receive detach messages.