Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Combine various ray plugins into one (issue 772) #778

Closed
wants to merge 3 commits into from
Closed

Combine various ray plugins into one (issue 772) #778

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d2bfdc0
Move gazebo_ros_laser to gazebo_ros_ray_sensor
Jul 20, 2018
7179e0a
Combine various ray plugins into one
Jul 23, 2018
0804470
Add sensor_msgs/Range output type to ray sensor
Jul 26, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 13 additions & 7 deletions gazebo_plugins/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -223,8 +223,20 @@ add_library(gazebo_ros_openni_kinect src/gazebo_ros_openni_kinect.cpp)
add_dependencies(gazebo_ros_openni_kinect ${PROJECT_NAME}_gencfg)
target_link_libraries(gazebo_ros_openni_kinect gazebo_ros_camera_utils DepthCameraPlugin ${catkin_LIBRARIES})

add_library(gazebo_ros_ray_sensor src/gazebo_ros_ray_sensor.cpp)
target_link_libraries(gazebo_ros_ray_sensor ${catkin_LIBRARIES})

# DEPRECATED instance of gazebo_ros_ray_sensor
add_library(gazebo_ros_laser src/gazebo_ros_laser.cpp)
target_link_libraries(gazebo_ros_laser gazebo_ros_ray_sensor ${catkin_LIBRARIES})

# DEPRECATED instance of gazebo_ros_ray_sensor
add_library(gazebo_ros_gpu_laser src/gazebo_ros_gpu_laser.cpp)
target_link_libraries(gazebo_ros_gpu_laser ${catkin_LIBRARIES} GpuRayPlugin)
target_link_libraries(gazebo_ros_gpu_laser gazebo_ros_laser ${catkin_LIBRARIES})

# DEPRECATED instance of gazebo_ros_ray_sensor
add_library(gazebo_ros_block_laser src/gazebo_ros_block_laser.cpp)
target_link_libraries(gazebo_ros_block_laser gazebo_ros_ray_sensor ${catkin_LIBRARIES})

if (NOT GAZEBO_VERSION VERSION_LESS 7.3)
add_library(gazebo_ros_harness src/gazebo_ros_harness.cpp)
Expand All @@ -233,12 +245,6 @@ if (NOT GAZEBO_VERSION VERSION_LESS 7.3)
${Boost_LIBRARIES} HarnessPlugin ${catkin_LIBRARIES})
endif()

add_library(gazebo_ros_laser src/gazebo_ros_laser.cpp)
target_link_libraries(gazebo_ros_laser RayPlugin ${catkin_LIBRARIES})

add_library(gazebo_ros_block_laser src/gazebo_ros_block_laser.cpp)
target_link_libraries(gazebo_ros_block_laser RayPlugin ${catkin_LIBRARIES})

add_library(gazebo_ros_p3d src/gazebo_ros_p3d.cpp)
target_link_libraries(gazebo_ros_p3d ${catkin_LIBRARIES} ${Boost_LIBRARIES})

Expand Down
101 changes: 7 additions & 94 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012-2014 Open Source Robotics Foundation
* Copyright 2013-2018 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
Expand All @@ -14,115 +14,28 @@
* limitations under the License.
*
*/
/*
* Desc: ros laser controller.
* Author: Nathan Koenig
* Date: 01 Feb 2007
*/

#ifndef GAZEBO_ROS_BLOCK_LASER_HH
#define GAZEBO_ROS_BLOCK_LASER_HH

// Custom Callback Queue
#include <ros/ros.h>
#include <ros/callback_queue.h>
#include <ros/advertise_options.h>

#include <sdf/Param.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/RayPlugin.hh>

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

#include <sensor_msgs/PointCloud.h>
#include <gazebo_plugins/gazebo_ros_ray_sensor.h>

namespace gazebo
{

class GazeboRosBlockLaser : public RayPlugin
/// Deprecated plugin, simply loads GazeboRosRaySensor with different configuration for backwards compatibility
class GazeboRosBlockLaser : public GazeboRosRaySensor
{
/// \brief Constructor
/// \param parent The parent entity, must be a Model or a Sensor
public: GazeboRosBlockLaser();

/// \brief Destructor
public: ~GazeboRosBlockLaser();

/// \brief Override the default behavior for resolving tf frame to provide backwards compatibility
protected: virtual std::string resolveTF(const std::string& _frame, const std::string& _robot_namespace, ros::NodeHandle& _nh) override;

/// \brief Load the plugin
/// \param take in SDF root element
public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);

/// \brief Update the controller
protected: virtual void OnNewLaserScans();

/// \brief Put laser data to the ROS topic
private: void PutLaserData(common::Time &_updateTime);

private: common::Time last_update_time_;

/// \brief Keep track of number of connctions
private: int laser_connect_count_;
private: void LaserConnect();
private: void LaserDisconnect();

// Pointer to the model
private: physics::WorldPtr world_;
/// \brief The parent sensor
private: sensors::SensorPtr parent_sensor_;
private: sensors::RaySensorPtr parent_ray_sensor_;

/// \brief pointer to ros node
private: ros::NodeHandle* rosnode_;
private: ros::Publisher pub_;

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

/// \brief topic name
private: std::string topic_name_;

/// \brief frame transform name, should match link name
private: std::string frame_name_;

/// \brief Gaussian noise
private: double gaussian_noise_;

/// \brief Gaussian noise generator
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;

/// \brief hack to mimic hokuyo intensity cutoff of 100
//private: ParamT<double> *hokuyoMinIntensityP;
private: double hokuyo_min_intensity_;

/// update rate of this sensor
private: double update_rate_;

/// \brief for setting ROS name space
private: std::string robot_namespace_;

// Custom Callback Queue
private: ros::CallbackQueue laser_queue_;
private: void LaserQueueThread();
private: boost::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);

};

}

#endif

77 changes: 8 additions & 69 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright 2012 Open Source Robotics Foundation
* Copyright 2013-2018 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
Expand All @@ -15,85 +15,24 @@
*
*/

#ifndef GAZEBO_ROS_LASER_HH
#define GAZEBO_ROS_LASER_HH
#ifndef GAZEBO_ROS_GPU_LASER_HH
#define GAZEBO_ROS_GPU_LASER_HH

#include <string>

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

#include <ros/ros.h>
#include <ros/advertise_options.h>
#include <sensor_msgs/LaserScan.h>

#include <gazebo/physics/physics.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/GpuRayPlugin.hh>

#include <sdf/sdf.hh>

#include <gazebo_plugins/PubQueue.h>
#include <gazebo_plugins/gazebo_ros_laser.h>

namespace gazebo
{
class GazeboRosLaser : public GpuRayPlugin
/// Deprecated plugin, simply loads GazeboRosRaySensor with different configuration for backwards compatibility
class GazeboRosGpuLaser : public GazeboRosLaser
{
/// \brief Constructor
public: GazeboRosLaser();
public: GazeboRosGpuLaser();

/// \brief Destructor
public: ~GazeboRosLaser();
public: ~GazeboRosGpuLaser();

/// \brief Load the plugin
/// \param take in SDF root element
public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);

/// \brief Keep track of number of connctions
private: int laser_connect_count_;
private: void LaserConnect();
private: void LaserDisconnect();

// Pointer to the model
private: std::string world_name_;
private: physics::WorldPtr world_;
/// \brief The parent sensor
private: sensors::GpuRaySensorPtr parent_ray_sensor_;

/// \brief pointer to ros node
private: ros::NodeHandle* rosnode_;
private: ros::Publisher pub_;
private: PubQueue<sensor_msgs::LaserScan>::Ptr pub_queue_;

/// \brief topic name
private: std::string topic_name_;

/// \brief frame transform name, should match link name
private: std::string frame_name_;

/// \brief tf prefix
private: std::string tf_prefix_;

/// \brief for setting ROS name space
private: std::string robot_namespace_;

// deferred load in case ros is blocking
private: sdf::ElementPtr sdf;
private: void LoadThread();
private: boost::thread deferred_load_thread_;
private: unsigned int seed;

private: gazebo::transport::NodePtr gazebo_node_;
private: gazebo::transport::SubscriberPtr laser_scan_sub_;
private: void OnScan(ConstLaserScanStampedPtr &_msg);

/// \brief prevents blocking
private: PubMultiQueue pmq;
};
}
#endif
73 changes: 7 additions & 66 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright 2012 Open Source Robotics Foundation
* Copyright 2013-2018 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
Expand All @@ -18,83 +18,24 @@
#ifndef GAZEBO_ROS_LASER_HH
#define GAZEBO_ROS_LASER_HH

#include <string>

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

#include <ros/ros.h>
#include <ros/advertise_options.h>
#include <sensor_msgs/LaserScan.h>

#include <sdf/Param.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <gazebo/msgs/MessageTypes.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/plugins/RayPlugin.hh>
#include <gazebo_plugins/gazebo_ros_utils.h>

#include <gazebo_plugins/PubQueue.h>
#include <gazebo_plugins/gazebo_ros_ray_sensor.h>

namespace gazebo
{
class GazeboRosLaser : public RayPlugin
/// Deprecated plugin, simply loads GazeboRosRaySensor with different configuration for backwards compatibility
class GazeboRosLaser : public GazeboRosRaySensor
{
/// \brief Constructor
public: GazeboRosLaser();

/// \brief Destructor
public: ~GazeboRosLaser();

/// \brief Override the default behavior for resolving tf frame to provide backwards compatibility
protected: virtual std::string resolveTF(const std::string& _frame, const std::string& _robot_namespace, ros::NodeHandle& _nh) override;

/// \brief Load the plugin
/// \param take in SDF root element
public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);

/// \brief Keep track of number of connctions
private: int laser_connect_count_;
private: void LaserConnect();
private: void LaserDisconnect();

// Pointer to the model
GazeboRosPtr gazebo_ros_;
private: std::string world_name_;
private: physics::WorldPtr world_;
/// \brief The parent sensor
private: sensors::RaySensorPtr parent_ray_sensor_;

/// \brief pointer to ros node
private: ros::NodeHandle* rosnode_;
private: ros::Publisher pub_;
private: PubQueue<sensor_msgs::LaserScan>::Ptr pub_queue_;

/// \brief topic name
private: std::string topic_name_;

/// \brief frame transform name, should match link name
private: std::string frame_name_;

/// \brief tf prefix
private: std::string tf_prefix_;

/// \brief for setting ROS name space
private: std::string robot_namespace_;

// deferred load in case ros is blocking
private: sdf::ElementPtr sdf;
private: void LoadThread();
private: boost::thread deferred_load_thread_;
private: unsigned int seed;

private: gazebo::transport::NodePtr gazebo_node_;
private: gazebo::transport::SubscriberPtr laser_scan_sub_;
private: void OnScan(ConstLaserScanStampedPtr &_msg);

/// \brief prevents blocking
private: PubMultiQueue pmq;
};
}
#endif
Loading