Initial version of the ROS wrapper for the depth camera ROS plugin.

481ba2dc · Sergi Hernandez · f2f14fc9 · 481ba2dc · 481ba2dc · 481ba2dc
Commit 481ba2dc authored 6 years ago by Sergi Hernandez
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+cmake_minimum_required(VERSION 2.8.3)
+project(iri_depth_camera_gazebo)
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES iri_depth_camera_gazebo
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+###########
+## Build ##
+###########
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/iri_depth_camera_gazebo.cpp
+# )
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/iri_depth_camera_gazebo_node.cpp)
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+#############
+## Install ##
+#############
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+#############
+## Testing ##
+#############
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_iri_depth_camera_gazebo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/README.md
+++ b/README.md
+# Description
+This ROS package encapsulates the IMU sensor from the [Hector gazebo plugins package](http://wiki.ros.org/hector_gazebo_plugins) in a xacro macro to simplify its integration. 
+# Dependencies
+This node has the following ROS dependencies:
+  * [Hector gazebo plugins package](http://wiki.ros.org/hector_gazebo_plugins)
+This dependencies can be installed with the following command:
+```
+sudo apt-get install ros-kinetic-hector-gazebo-plugins
+```
+This node has the following ROS IRI dependencies:
+  * [IRI Microstrain IMU description](https://gitlab.iri.upc.edu/labrobotica/ros/sensors/imu/iri_microstrain_imu_description).
+# Install
+This package can be cloned to an active workspace with the following command:
+```
+roscd
+cd ../src
+git clone https://gitlab.iri.upc.edu/labrobotica/ros/sensors/imu/iri_imu_gazebo.git 
+```
+# How to use it
+This IMU gazebo model is automatically included when an IMU sensor in included in a xacro or urdf file. This plugin can be fully configured by a YAML file which is provided as a parameter to the xacro macro.
+A separate YAML file is provided for each IMU sensor. At the moment the supported sensors are:
+ * Microstrain 3dm-gx3-25 
--- a/config/realsense_d435_depth_sim_config.yaml
+++ b/config/realsense_d435_depth_sim_config.yaml
+rate: 60.0
+horizontal_fov: 1.047
+image_width: 640
+image_height: 480
+image_format: 'R8G8B8'
+clip_near: 0.2
+clip_far: 100.0
+noise_type: 'gaussian'
+noise_mean: 0.0
+noise_stddev: 0.007
+image_topic_name: '/camera/color/image_raw'
+camera_info_topic_name: '/camera/color/camera_info'
+depth_image_topic_name: '/camera/depth/image_rect_raw'
+point_cloud_topic_name: '/camera/depth_registered/points'
+depth_image_camera_info_topic_name: '/camera/depth/camera_info'
+point_cloud_cut_off: 0.4
+hack_baseline: 0.007
+cx_prime: 0.0
+cx: 0.0
+cy: 0.0
+focal_length: 0.0
--- a/launch/realsense_d435_depth_sim.launch
+++ b/launch/realsense_d435_depth_sim.launch
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="node_name" default="camera" />
+  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="extra_gazebo_args" value=""/>
+    <arg name="gui" value="true"/>
+    <arg name="recording" value="false"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+    <arg name="physics" value="ode"/>
+    <arg name="verbose" value="false"/>
+    <arg name="world_name" value="worlds/empty.world"/> 
+    <arg name="respawn_gazebo" value="false"/>
+    <arg name="use_clock_frequency" value="false"/>
+    <arg name="pub_clock_frequency" value="100"/>
+  </include>
+  <param name="robot_description"
+         command="$(find xacro)/xacro --inorder '$(find iri_realsense_camera_description)/urdf/realsense_example.xacro' name:=$(arg node_name)" />
+  <node pkg="robot_state_publisher" type="state_publisher" name="realsense_tf_broadcaster">
+    <param name="tf_prefix" type="string" value="/$(arg node_name)"/>
+    <param name="publish_frequency" type="double" value="20.0"/>
+  </node>
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model realsense_depth -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" />
+</launch>
--- a/package.xml
+++ b/package.xml
+<?xml version="1.0"?>
+<package format="2">
+  <name>iri_depth_camera_gazebo</name>
+  <version>0.0.0</version>
+  <description>The iri_depth_camera_gazebo package</description>
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer>
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>LGPL</license>
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/iri_depth_camera_gazebo</url> -->
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <author email="labrobotica@iri.upc.edu">labrobotica</author>
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>iri_realsense_depth_description</depend>
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
--- a/urdf/realsense.gazebo
+++ b/urdf/realsense.gazebo
+<?xml version="1.0"?>
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="iri_depth_camera_gazebo" params="name config">
+    <xacro:property name="properties" value="${load_yaml(config)}"/>
+    <gazebo reference="${name}_link">
+      <sensor type="depth" name="${name}">
+        <always_on>1</always_on>
+        <visualize>true</visualize>
+        <update_rate>${properties['rate']}</update_rate>
+        <camera>
+          <horizontal_fov>${properties['horizontal_fov']}</horizontal_fov>
+          <image>
+            <width>${properties['image_width']}</width>
+            <height>${properties['image_height']}</height>
+            <format>${properties['image_format']}</format>
+          </image>
+          <depth_camera>
+          </depth_camera>
+          <clip>
+            <near>${properties['clip_near']}</near>
+            <far>${properties['clip_far']}</far>
+          </clip>
+          <noise>
+            <type>${properties['noise_type']}</type>
+            <mean>${properties['noise_mean']}</mean>
+            <stddev>${properties['noise_stddev']}</stddev>
+          </noise>
+        </camera>
+        <plugin name="${name}_controller" filename="libgazebo_ros_openni_kinect.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>${properties['rate']}</updateRate>
+          <cameraName>${name}</cameraName>
+          <frameName>${name}_color_optical_frame</frameName>
+          <imageTopicName>${properties['image_topic_name']}</imageTopicName>
+          <cameraInfoTopicName>${properties['camera_info_topic_name']}</cameraInfoTopicName>
+          <depthImageTopicName>${properties['depth_image_topic_name']}</depthImageTopicName>
+          <pointCloudTopicName>${properties['point_cloud_topic_name']}</pointCloudTopicName>
+          <depthImageCameraInfoTopicName>${properties['depth_image_camera_info_topic_name']}</depthImageCameraInfoTopicName>
+          <pointCloudCutoff>${properties['point_cloud_cut_off']}</pointCloudCutoff>
+          <hackBaseline>${properties['hack_baseline']}</hackBaseline>
+          <CxPrime>${properties['cx_prime']}</CxPrime>
+          <Cx>${properties['cx']}</Cx>
+          <Cy>${properties['cy']}</Cy>
+          <focalLength>${properties['focal_length']}</focalLength>
+          <noise>
+            <type>${properties['noise_type']}</type>
+            <mean>${properties['noise_mean']}</mean>
+            <stddev>${properties['noise_stddev']}</stddev>
+          </noise>
+        </plugin>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+</root>