Skip to content
Permalink
Browse files

Reference issue ros-simulation#512

  • Loading branch information...
vdiluoffo committed Jan 9, 2017
1 parent e97433a commit ded1a82070d873b92795346a7cac92acb9698e82
Showing with 714 additions and 419 deletions.
  1. +65 −29 gazebo_plugins/include/gazebo_plugins/PubQueue.h
  2. +26 −15 gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h
  3. +18 −10 gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h
  4. +43 −26 gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
  5. +19 −16 gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h
  6. +30 −19 gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h
  7. +15 −9 gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h
  8. +23 −12 gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h
  9. +16 −10 gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h
  10. +20 −11 gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h
  11. +17 −9 gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h
  12. +9 −4 gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h
  13. +20 −12 gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h
  14. +28 −18 gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h
  15. +30 −20 gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h
  16. +11 −6 gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h
  17. +28 −19 gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_trajectory.h
  18. +16 −9 gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h
  19. +6 −3 gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h
  20. +23 −17 gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h
  21. +23 −13 gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h
  22. +31 −17 gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h
  23. +22 −14 gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h
  24. +8 −6 gazebo_plugins/include/gazebo_plugins/gazebo_ros_prosilica.h
  25. +26 −15 gazebo_plugins/include/gazebo_plugins/gazebo_ros_range.h
  26. +26 −16 gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h
  27. +3 −1 gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h
  28. +32 −21 gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h
  29. +15 −6 gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h
  30. +24 −13 gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h
  31. +21 −12 gazebo_plugins/include/gazebo_plugins/gazebo_ros_video.h
  32. +20 −11 gazebo_plugins/include/gazebo_plugins/vision_reconfigure.h
@@ -18,13 +18,17 @@
#ifndef ROS_PUBQUEUE_H
#define ROS_PUBQUEUE_H

#include <boost/thread.hpp>
#include <boost/bind.hpp>
/#include <boost/bind.hpp>
#include <functional>
//#include <boost/thread.hpp>
#include <thread>
#include <deque>
#include <list>
#include <vector>

#include <ros/ros.h>
//#include <ros/ros.h>
#include "rcl/rcl.h"
#include "rclcpp/rclcpp.hpp"


/// \brief Container for a (ROS publisher, outgoing message) pair.
@@ -36,7 +40,8 @@ class PubMessagePair
/// \brief The outgoing message.
T msg_;
/// \brief The publisher to use to publish the message.
ros::Publisher pub_;
// ros::Publisher pub_;
rclcpp::Publisher pub_;
PubMessagePair(T& msg, ros::Publisher& pub) :
msg_(msg), pub_(pub) {}
};
@@ -48,22 +53,28 @@ template<class T>
class PubQueue
{
public:
typedef boost::shared_ptr<std::deque<boost::shared_ptr<
// typedef boost::shared_ptr<std::deque<boost::shared_ptr<
typedef std::shared_ptr<std::deque<boost::shared_ptr<
PubMessagePair<T> > > > QueuePtr;
typedef boost::shared_ptr<PubQueue<T> > Ptr;
// typedef boost::shared_ptr<PubQueue<T> > Ptr;
typedef std::shared_ptr<PubQueue<T> > Ptr;

private:
/// \brief Our queue of outgoing messages.
QueuePtr queue_;
/// \brief Mutex to control access to the queue.
boost::shared_ptr<boost::mutex> queue_lock_;
// boost::shared_ptr<boost::mutex> queue_lock_;
std::shared_ptr<boost::mutex> queue_lock_;
/// \brief Function that will be called when a new message is pushed on.
boost::function<void()> notify_func_;
// boost::function<void()> notify_func_;
std::function<void()> notify_func_;

public:
PubQueue(QueuePtr queue,
boost::shared_ptr<boost::mutex> queue_lock,
boost::function<void()> notify_func) :
// boost::shared_ptr<boost::mutex> queue_lock,
// boost::function<void()> notify_func) :
std::shared_ptr<boost::mutex> queue_lock,
std::function<void()> notify_func) :
queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {}
~PubQueue() {}

@@ -72,8 +83,10 @@ class PubQueue
/// \param[in] pub The ROS publisher to use to publish the message
void push(T& msg, ros::Publisher& pub)
{
boost::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
boost::mutex::scoped_lock lock(*queue_lock_);
// boost::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
// boost::mutex::scoped_lock lock(*queue_lock_);
std::shared_ptr<PubMessagePair<T> > el(new PubMessagePair<T>(msg, pub));
std::mutex::scoped_lock lock(*queue_lock_);
queue_->push_back(el);
notify_func_();
}
@@ -82,7 +95,8 @@ class PubQueue
/// \param[out] els Place to store the popped messages
void pop(std::vector<boost::shared_ptr<PubMessagePair<T> > >& els)
{
boost::mutex::scoped_lock lock(*queue_lock_);
// boost::mutex::scoped_lock lock(*queue_lock_);
std::mutex::scoped_lock lock(*queue_lock_);
while(!queue_->empty())
{
els.push_back(queue_->front());
@@ -99,24 +113,33 @@ class PubMultiQueue
/// \brief List of functions to be called to service our queues.
std::list<boost::function<void()> > service_funcs_;
/// \brief Mutex to lock access to service_funcs_
boost::mutex service_funcs_lock_;
// boost::mutex service_funcs_lock_;
/// \brief If started, the thread that will call the service functions
boost::thread service_thread_;
// boost::thread service_thread_;
/// \brief Boolean flag to shutdown the service thread if PubMultiQueue is destructed
bool service_thread_running_;
/// \brief Condition variable used to block and resume service_thread_
boost::condition_variable service_cond_var_;
/// \brief Mutex to accompany service_cond_var_
boost::mutex service_cond_var_lock_;
// boost::mutex service_cond_var_lock_;
std::mutex service_funcs_lock_;
/// \brief If started, the thread that will call the service functions
std::thread service_thread_;
/// \brief Mutex to accompany service_cond_var_
std::mutex service_cond_var_lock_;


/// \brief Service a given queue by popping outgoing message off it and
/// publishing them.
template <class T>
void serviceFunc(boost::shared_ptr<PubQueue<T> > pq)
// void serviceFunc(boost::shared_ptr<PubQueue<T> > pq)
void serviceFunc(std::shared_ptr<PubQueue<T> > pq)
{
std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
// std::vector<boost::shared_ptr<PubMessagePair<T> > > els;
std::vector<std::shared_ptr<PubMessagePair<T> > > els;
pq->pop(els);
for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
// for(typename std::vector<boost::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
for(typename std::vector<std::shared_ptr<PubMessagePair<T> > >::iterator it = els.begin();
it != els.end();
++it)
{
@@ -140,14 +163,22 @@ class PubMultiQueue
/// least each type of publish message).
/// \return Pointer to the newly created queue, good for calling push() on.
template <class T>
boost::shared_ptr<PubQueue<T> > addPub()
// boost::shared_ptr<PubQueue<T> > addPub()
std::shared_ptr<PubQueue<T> > addPub()
{
typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
// typename PubQueue<T>::QueuePtr queue(new std::deque<boost::shared_ptr<PubMessagePair<T> > >);
// boost::shared_ptr<boost::mutex> queue_lock(new boost::mutex);
// boost::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this)));
// boost::function<void()> f = boost::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
// {
// boost::mutex::scoped_lock lock(service_funcs_lock_);

typename PubQueue<T>::QueuePtr queue(new std::deque<std::shared_ptr<PubMessagePair<T> > >);
std::shared_ptr<std::mutex> queue_lock(new std::mutex);
std::shared_ptr<PubQueue<T> > pq(new PubQueue<T>(queue, queue_lock, std::bind(&PubMultiQueue::notifyServiceThread, this)));
std::function<void()> f = std::bind(&PubMultiQueue::serviceFunc<T>, this, pq);
{
boost::mutex::scoped_lock lock(service_funcs_lock_);
std::mutex::scoped_lock lock(service_funcs_lock_);
service_funcs_.push_back(f);
}
return pq;
@@ -156,8 +187,11 @@ class PubMultiQueue
/// \brief Service each queue one time.
void spinOnce()
{
boost::mutex::scoped_lock lock(service_funcs_lock_);
for(std::list<boost::function<void()> >::iterator it = service_funcs_.begin();
// boost::mutex::scoped_lock lock(service_funcs_lock_);
// for(std::list<boost::function<void()> >::iterator it = service_funcs_.begin();

std::mutex::scoped_lock lock(service_funcs_lock_);
for(std::list<std::function<void()> >::iterator it = service_funcs_.begin();
it != service_funcs_.end();
++it)
{
@@ -171,7 +205,8 @@ class PubMultiQueue
{
while(ros::ok() && service_thread_running_)
{
boost::unique_lock<boost::mutex> lock(service_cond_var_lock_);
// boost::unique_lock<boost::mutex> lock(service_cond_var_lock_);
std::unique_lock<std::mutex> lock(service_cond_var_lock_);
service_cond_var_.wait(lock);
spinOnce();
}
@@ -181,7 +216,8 @@ class PubMultiQueue
void startServiceThread()
{
service_thread_running_ = true;
service_thread_ = boost::thread(boost::bind(&PubMultiQueue::spin, this));
// service_thread_ = boost::thread(boost::bind(&PubMultiQueue::spin, this));
service_thread_ = std::thread(boost::bind(&PubMultiQueue::spin, this));
}

/// \brief Wake up the queue serive thread (e.g., after having pushed a
@@ -24,9 +24,11 @@
#define GAZEBO_ROS_BLOCK_LASER_HH

// Custom Callback Queue
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
//#include <ros/ros.h>
#include "rcl/rcl.h"
#include "rclcpp/rclcpp.hpp"
//#include <ros/callback_queue.h>
//#include <ros/advertise_options.h>

#include <sdf/Param.hh>
#include <gazebo/physics/physics.hh>
@@ -37,11 +39,14 @@
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/RayPlugin.hh>

#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
//#include <boost/bind.hpp>
#include <functional>
//#include <boost/thread.hpp>
#include <thread>
//#include <boost/thread/mutex.hpp>
#include <mutex>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/msg/PointCloud.h>

namespace gazebo
{
@@ -79,11 +84,13 @@ namespace gazebo
private: sensors::RaySensorPtr parent_ray_sensor_;

/// \brief pointer to ros node
private: ros::NodeHandle* rosnode_;
private: ros::Publisher pub_;
// private: ros::NodeHandle* rosnode_;
private: rcl_node_t * rcl_node; // or should the rclcpp::Node::shared_ptr be used?
// private: ros::Publisher pub_;
private rclcpp::Publisher pub_;

/// \brief ros message
private: sensor_msgs::PointCloud cloud_msg_;
private: sensor_msgs::msg::PointCloud cloud_msg_;

/// \brief topic name
private: std::string topic_name_;
@@ -98,8 +105,9 @@ namespace gazebo
private: double GaussianKernel(double mu,double sigma);

/// \brief A mutex to lock access to fields that are used in message callbacks
private: boost::mutex lock;

// private: boost::mutex lock;
private: std::mutex lock;

/// \brief hack to mimic hokuyo intensity cutoff of 100
//private: ParamT<double> *hokuyoMinIntensityP;
private: double hokuyo_min_intensity_;
@@ -111,14 +119,17 @@ namespace gazebo
private: std::string robot_namespace_;

// Custom Callback Queue
private: ros::CallbackQueue laser_queue_;
// private: ros::CallbackQueue laser_queue_;
private: callback laser_queue_;
private: void LaserQueueThread();
private: boost::thread callback_laser_queue_thread_;
// private: boost::thread callback_laser_queue_thread_;
private: std::thread callback_laser_queue_thread_;

// subscribe to world stats
private: transport::NodePtr node_;
private: common::Time sim_time_;
public: void OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg);
// public: void OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg);
public: void OnStats( const std::shared_ptr<msgs::WorldStatistics const> &_msg);

};

@@ -20,16 +20,20 @@

#include <string>

#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
//#include <ros/ros.h>
#include "rcl/rcl.h"
#include "rclcpp/rclcpp.hpp"
//#include <ros/callback_queue.h>
//#include <ros/advertise_options.h>

#include <sys/time.h>

#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
//#include <boost/thread.hpp>
#include <thread>
//#include <boost/thread/mutex.hpp>
#include <mutex>

#include <std_msgs/String.h>
#include <std_msgs/msg/String.h>

#include <gazebo_msgs/ContactState.h>
#include <gazebo_msgs/ContactsState.h>
@@ -64,8 +68,10 @@ namespace gazebo
private: void OnContact();

/// \brief pointer to ros node
private: ros::NodeHandle* rosnode_;
private: ros::Publisher contact_pub_;
// private: ros::NodeHandle* rosnode_;
rcl_node_t * rcl_node;
// private: ros::Publisher contact_pub_;
private: rclcpp::Publisher contact_pub_;

private: sensors::ContactSensorPtr parentSensor;

@@ -80,9 +86,11 @@ namespace gazebo
/// \brief for setting ROS name space
private: std::string robot_namespace_;

private: ros::CallbackQueue contact_queue_;
// private: ros::CallbackQueue contact_queue_;
private: callback contact_queue_;;
private: void ContactQueueThread();
private: boost::thread callback_queue_thread_;
// private: boost::thread callback_queue_thread_;
private: std::thread callback_queue_thread_;

// Pointer to the update event connection
private: event::ConnectionPtr update_connection_;

0 comments on commit ded1a82

Please sign in to comment.
You can’t perform that action at this time.