Merge pull request #13 from Field-Robotics-Lab/depth_camera_sonar

Depth camera sonar

.gitignore

@@ -34,3 +34,5 @@
 # VIM
 *.swp
 
+# Python
+*.pyc

CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(nps_uw_sensors_gazebo)
 
-find_package(catkin REQUIRED tf)
+find_package(catkin REQUIRED tf gazebo_plugins)
 
 find_package(gazebo REQUIRED)
 find_package(roscpp REQUIRED)
@@ -22,16 +22,25 @@ catkin_package()
 ## Plugins
 
 add_library(gazebo_ros_pulse_lidar_plugin src/gazebo_ros_pulse_lidar.cpp)
+target_link_libraries(gazebo_ros_pulse_lidar_plugin 
+                      ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES})
+
 add_library(nps_gazebo_ros_gpu_sonar_single_beam_plugin 
             src/nps_gazebo_ros_gpu_sonar_single_beam.cpp)
-
 target_link_libraries(nps_gazebo_ros_gpu_sonar_single_beam_plugin
-                      GpuRayPlugin ${catkin_LIBRARIES})
-target_link_libraries(gazebo_ros_pulse_lidar_plugin 
-                      ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES})
+                      GpuRayPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
+add_library(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin
+            src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp
+            include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh)
+target_link_libraries(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin
+                      DepthCameraPlugin ${catkin_LIBRARIES})
 
 # for launch
 install(DIRECTORY launch
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
         PATTERN "*~" EXCLUDE)
 
+# for Python scripts
+catkin_install_python(PROGRAMS scripts/depth_camera_sonar.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh

@@ -0,0 +1,184 @@
+/*
+ * Copyright (C) 2012-2014 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.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+/*
+ * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor.
+ * Author: John Hsu
+ * Date: 24 Sept 2008
+ */
+
+#ifndef GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH
+#define GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH
+
+// ros stuff
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <ros/advertise_options.h>
+
+// ros messages stuff
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/fill_image.h>
+#include <std_msgs/Float64.h>
+#include <image_transport/image_transport.h>
+
+// dynamic reconfigure stuff
+#include <gazebo_plugins/GazeboRosCameraConfig.h>
+#include <dynamic_reconfigure/server.h>
+
+// boost stuff
+#include <boost/thread/mutex.hpp>
+
+// camera stuff
+#include <gazebo_plugins/gazebo_ros_camera_utils.h>
+
+// gazebo stuff
+#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/sensors/SensorTypes.hh>
+#include <gazebo/plugins/DepthCameraPlugin.hh>
+
+#include <string>
+
+namespace gazebo
+{
+  class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils
+  {
+    /// \brief Constructor
+    /// \param parent The parent entity, must be a Model or a Sensor
+    public: GazeboRosDepthCamera();
+
+    /// \brief Destructor
+    public: ~GazeboRosDepthCamera();
+
+    /// \brief Load the plugin
+    /// \param take in SDF root element
+    public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
+
+    /// \brief Advertise point cloud and depth image
+    public: virtual void Advertise();
+
+    /// \brief Update the controller
+    protected: virtual void OnNewDepthFrame(const float *_image,
+                   unsigned int _width, unsigned int _height,
+                   unsigned int _depth, const std::string &_format);
+
+    /// \brief Update the controller
+    protected: virtual void OnNewNormalsFrame(const float *_image,
+                   unsigned int _width, unsigned int _height,
+                   unsigned int _depth, const std::string &_format);
+
+    /// \brief Update the controller
+    protected: virtual void OnNewRGBPointCloud(const float *_pcd,
+                    unsigned int _width, unsigned int _height,
+                    unsigned int _depth, const std::string &_format);
+
+    /// \brief Update the controller
+    protected: virtual void OnNewImageFrame(const unsigned char *_image,
+                   unsigned int _width, unsigned int _height,
+                   unsigned int _depth, const std::string &_format);
+
+    /// \brief Put camera data to the ROS topic
+    private: void FillPointdCloud(const float *_src);
+
+    /// \brief push depth image data into ros topic
+    private: void FillDepthImage(const float *_src);
+
+    /// \brief push normals image data into ros topic
+    private: void FillNormalsImage(const float *_src);
+
+    /// \brief Keep track of number of connctions for point clouds
+    private: int point_cloud_connect_count_;
+    private: void PointCloudConnect();
+    private: void PointCloudDisconnect();
+
+    /// \brief Keep track of number of connctions for Sonar point clouds
+    private: int sonar_point_cloud_connect_count_;
+    private: void SonarPointCloudConnect();
+    private: void SonarPointCloudDisconnect();
+
+    /// \brief Keep track of number of connctions for images
+    private: int depth_image_connect_count_;
+    private: void DepthImageConnect();
+    private: void DepthImageDisconnect();
+    private: int normals_image_connect_count_;
+    private: void NormalsImageConnect();
+    private: void NormalsImageDisconnect();
+    private: void RayImageConnect();
+    private: void RayImageDisconnect();
+    private: common::Time last_depth_image_camera_info_update_time_;
+
+    private: bool FillPointCloudHelper(
+                                  sensor_msgs::PointCloud2 &point_cloud_msg,
+                                  uint32_t rows_arg, uint32_t cols_arg,
+                                  uint32_t step_arg, void* data_arg);
+
+    private: bool FillDepthImageHelper(sensor_msgs::Image& image_msg,
+                                  uint32_t rows_arg, uint32_t cols_arg,
+                                  uint32_t step_arg, void* data_arg);
+
+    private: bool FillNormalsImageHelper(sensor_msgs::Image& image_msg,
+                                  uint32_t rows_arg, uint32_t cols_arg,
+                                  uint32_t step_arg, void* data_arg);
+
+    /// \brief A pointer to the ROS node.
+    /// A node will be instantiated if it does not exist.
+    private: ros::Publisher point_cloud_pub_;
+
+    private: ros::Publisher depth_image_pub_;
+    private: ros::Publisher normals_image_pub_;
+
+    /// \brief PointCloud2 point cloud message
+    private: sensor_msgs::PointCloud2 point_cloud_msg_;
+
+    private: sensor_msgs::Image depth_image_msg_;
+    private: sensor_msgs::Image normals_image_msg_;
+
+    private: double point_cloud_cutoff_;
+
+    /// \brief ROS image topic name
+    private: std::string point_cloud_topic_name_;
+
+    private: void InfoConnect();
+    private: void InfoDisconnect();
+
+    using GazeboRosCameraUtils::PublishCameraInfo;
+    protected: virtual void PublishCameraInfo();
+
+    /// \brief image where each pixel contains the depth information
+    private: std::string depth_image_topic_name_;
+    private: std::string normals_image_topic_name_;
+    private: std::string depth_image_camera_info_topic_name_;
+    private: int depth_info_connect_count_;
+    private: void DepthInfoConnect();
+    private: void DepthInfoDisconnect();
+
+    // overload with our own
+    private: common::Time depth_sensor_update_time_;
+    private: common::Time normals_sensor_update_time_;
+    protected: ros::Publisher depth_image_camera_info_pub_;
+
+    private: event::ConnectionPtr load_connection_;
+  };
+
+}
+#endif
+
+

launch/depth_camera_sonar_basic.launch

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find nps_uw_sensors_gazebo)/worlds/depth_camera_sonar_basic.world"/>
+    <arg name="verbose" value="true"/>
+  </include>
+
+<!-- depth_camera_sonar_robot xacro -->
+  <param name="depth_camera_sonar_robot"
+         command="$(find xacro)/xacro '$(find nps_uw_sensors_gazebo)/urdf/depth_camera_sonar_robot.xacro'"/>
+
+<!-- spawn depth_camera_sonar_robot -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+        respawn="false" output="screen"
+        args="-urdf -model nps_depth_camera_sonar_model -param depth_camera_sonar_robot" />
+
+    <!-- this Python ROS node converts depth and normals to Sonar outputs -->
+    <node name="depth_and_normals_to_sonar" pkg="nps_uw_sensors_gazebo"
+          type="sonar_ros.py">
+    </node>
+
+    <!-- rqt_image_view for sonar_beam_image -->
+    <node name="rqt_image_view_image_beams" pkg="rqt_image_view"
+          type="rqt_image_view" args="/sonar_beam_image">
+    </node>
+
+</launch>
+