updating for hydro: major refactor and fixes

camera_pose/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(camera_pose)
+find_package(catkin REQUIRED)
+catkin_metapackage()

camera_pose/package.xml

@@ -0,0 +1,38 @@
+<package>
+  <name>camera_pose</name>
+  <version>0.5.0</version>
+  <description>This stack contains a set of tools to work with multiple rgb
+    camera's simultaneoulsy.</description>
+  <maintainer email="[email protected]">Vijay Pradeep</maintainer>
+  <maintainer email="[email protected]">Wim Meeussen</maintainer>
+  <maintainer email="[email protected]">Yiping Liu</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/camera_pose</url>
+  <!-- <url type="bugtracker"></url> -->
+
+  <author>Vijay Pradeep</author>
+  <author>Wim Meeussen</author>
+  <author>Yiping Liu</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>camera_pose_calibration</run_depend>
+  <run_depend>camera_pose_toolkits</run_depend>
+  <run_depend>kinect_depth_calibration</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+
+
+
+
+
+
+</package>

camera_pose_calibration/CMakeLists.txt

@@ -1,5 +1,17 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+# Catkin User Guide: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/user_guide.html
+# Catkin CMake Standard: http://www.ros.org/doc/groovy/api/catkin/html/user_guide/standards.html
+cmake_minimum_required(VERSION 2.8.3)
+project(camera_pose_calibration)
+# Load catkin and all dependencies required for this package
+# TODO: remove all from COMPONENTS that are not catkin packages.
+find_package(catkin REQUIRED COMPONENTS geometry_msgs calibration_msgs sensor_msgs image_cb_detector monocam_settler interval_intersection joint_states_settler tf_conversions tf2_ros tf2_kdl message_filters)
+
+# include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+# CATKIN_MIGRATION: removed during catkin migration
+# cmake_minimum_required(VERSION 2.4.6)
+
+# CATKIN_MIGRATION: removed during catkin migration
+# include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 
 # Set the build type.  Options are:
 #  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
@@ -9,26 +21,52 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
 #set(ROS_BUILD_TYPE RelWithDebInfo)
 
-rosbuild_init()
 
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+# CATKIN_MIGRATION: removed during catkin migration
+# rosbuild_init()
 
 #uncomment if you have defined messages
-rosbuild_genmsg()
+add_message_files(
+  FILES
+  CalibrationEstimate.msg
+  CameraCalibration.msg
+  CameraMeasurement.msg
+  CameraPose.msg
+  RobotMeasurement.msg
+)
 #uncomment if you have defined services
-rosbuild_gensrv()
+add_service_files(
+  FILES
+  TriggerOne.srv
+)
 
 #common commands for building c++ executables and libraries
-#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#add_library(${PROJECT_NAME} src/example.cpp)
 #target_link_libraries(${PROJECT_NAME} another_library)
-#rosbuild_add_boost_directories()
-#rosbuild_link_boost(${PROJECT_NAME} thread)
-#rosbuild_add_executable(example examples/example.cpp)
+#
+# CATKIN_MIGRATION: removed during catkin migration
+# rosbuild_add_boost_directories()
+#find_package(Boost REQUIRED COMPONENTS thread)
+#include_directories(${Boost_INCLUDE_DIRS})
+#target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES})
+#add_executable(example examples/example.cpp)
 #target_link_libraries(example ${PROJECT_NAME})
 
-#rosbuild_download_test_data(http://pr.willowgarage.com/data/camera_pose_calibration/camera_pose_cal_regression_data.bag test/camera_pose_cal_regression_data.bag fcd96a5c7a74a064640753fd9d82fff3)
-#rosbuild_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_camera_pose_calibration.launch)
+#download_test_data(http://pr.willowgarage.com/data/camera_pose_calibration/camera_pose_cal_regression_data.bag test/camera_pose_cal_regression_data.bag fcd96a5c7a74a064640753fd9d82fff3)
+#add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_camera_pose_calibration.launch)
+
+catkin_python_setup()
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES calibration_msgs geometry_msgs sensor_msgs std_msgs
+)
 
+# catkin_package parameters: http://ros.org/doc/groovy/api/catkin/html/dev_guide/generated_cmake_api.html#catkin-package
+# TODO: fill in what other packages will need to use this package
+catkin_package(
+    DEPENDS geometry_msgs calibration_msgs sensor_msgs image_cb_detector monocam_settler interval_intersection joint_states_settler kdl tf_conversions tf2_ros tf2_kdl message_filters opencv2
+    CATKIN_DEPENDS # TODO
+    INCLUDE_DIRS # TODO include
+    LIBRARIES # TODO
+)

camera_pose_calibration/blocks/rgb_block.launch

@@ -3,14 +3,15 @@
   <arg name="checker_rows" />
   <arg name="checker_cols" />
   <arg name="checker_size" />
+  <arg name="image_topic" />
 
 
   <group ns="$(arg ns)">
     <!-- Throttle images -->
-    <node type="throttle" pkg="topic_tools" name="image_throttle" args="messages image_rect 20 image_throttle" />
+    <node type="throttle" pkg="topic_tools" name="image_throttle" args="messages $(arg image_topic) 20 image_throttle" />
 
     <!-- Checkerboard detector -->
-    <node pkg="image_cb_detector" type="image_cb_detector_action_old" name="stereo_cb_detector" output="screen">
+    <node pkg="image_cb_detector" type="image_cb_detector_action" name="stereo_cb_detector" output="screen">
       <remap from="image" to="image_throttle" />
       <remap from="camera_info" to="camera_info" />
     </node>
@@ -21,4 +22,4 @@
     <node type="monocam_settler_action" pkg="monocam_settler" name="monocam_settler" output="screen"/>
     <node pkg="camera_pose_calibration" type="start_monocam_settler_action.py" name="start_monocam_settler_action" />
   </group>
-</launch>
+</launch>

camera_pose_calibration/launch_extrinsics/calibrate_2_camera.launch

@@ -1,6 +1,8 @@
 <launch>
   <arg name="camera1_ns" default="camera_a/rgb" />
+  <arg name="camera1_image" default="image_rect" />
   <arg name="camera2_ns" default="camera_b/rgb" />
+  <arg name="camera2_image" default="image_rect" />
   <arg name="checker_rows" default="4"/>
   <arg name="checker_cols" default="5"/>
   <arg name="checker_size" default="0.0245"/>
@@ -9,12 +11,14 @@
   <!-- checkerboard detector for each camera -->
   <include file="$(find camera_pose_calibration)/blocks/rgb_block.launch">
     <arg name="ns" value="$(arg camera1_ns)" />
+    <arg name="image_topic" value="$(arg camera1_image)" />
     <arg name="checker_rows" value="$(arg checker_rows)" />
     <arg name="checker_cols" value="$(arg checker_cols)" />
     <arg name="checker_size" value="$(arg checker_size)" />
   </include>
   <include file="$(find camera_pose_calibration)/blocks/rgb_block.launch">
     <arg name="ns" value="$(arg camera2_ns)" />
+    <arg name="image_topic" value="$(arg camera2_image)" />
     <arg name="checker_rows" value="$(arg checker_rows)" />
     <arg name="checker_cols" value="$(arg checker_cols)" />
     <arg name="checker_size" value="$(arg checker_size)" />
@@ -28,6 +32,7 @@
   <node pkg="camera_pose_calibration" type="filter_intervals.py" name="filter_intervals">
      <param name="min_duration" value="1.0" />
      <remap from="features" to="$(arg camera1_ns)/features" />
+     <remap from="interval" to="intersected_interval"/>
   </node>
 
 

camera_pose_calibration/package.xml

@@ -0,0 +1,66 @@
+<package>
+  <name>camera_pose_calibration</name>
+  <version>0.5.0</version>
+  <description>This package provides the pipeline to calibrate the relative 6D
+    poses between multiple camera's. The calibration can be preformed
+    online, and the results are published to tf in realtime.</description>
+  <maintainer email="[email protected]">Vijay Pradeep</maintainer>
+  <maintainer email="[email protected]">Wim Meeussen</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/camera_pose_calibration</url>
+  <!-- <url type="bugtracker"></url> -->
+
+  <author>Vijay Pradeep</author>
+  <author>Wim Meeussen</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>calibration_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_cb_detector</build_depend>
+  <build_depend>monocam_settler</build_depend>
+  <build_depend>interval_intersection</build_depend>
+  <build_depend>joint_states_settler</build_depend>
+  <build_depend>kdl</build_depend>
+  <build_depend>tf_conversions</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_kdl</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>opencv2</build_depend>
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>calibration_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_cb_detector</run_depend>
+  <run_depend>monocam_settler</run_depend>
+  <run_depend>interval_intersection</run_depend>
+  <run_depend>joint_states_settler</run_depend>
+  <run_depend>kdl</run_depend>
+  <run_depend>tf_conversions</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_kdl</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>opencv2</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+  <!-- <test_depend>geometry_msgs</test_depend> -->
+  <!-- <test_depend>calibration_msgs</test_depend> -->
+  <!-- <test_depend>sensor_msgs</test_depend> -->
+  <!-- <test_depend>image_cb_detector</test_depend> -->
+  <!-- <test_depend>monocam_settler</test_depend> -->
+  <!-- <test_depend>interval_intersection</test_depend> -->
+  <!-- <test_depend>joint_states_settler</test_depend> -->
+  <!-- <test_depend>kdl</test_depend> -->
+  <!-- <test_depend>tf_conversions</test_depend> -->
+  <!-- <test_depend>tf2_ros</test_depend> -->
+  <!-- <test_depend>tf2_kdl</test_depend> -->
+  <!-- <test_depend>message_filters</test_depend> -->
+  <!-- <test_depend>opencv2</test_depend> -->
+
+</package>

camera_pose_calibration/src/camera_pose_calibration/calibration_transform_publisher.py → camera_pose_calibration/scripts/calibration_transform_publisher.py

@@ -1,6 +1,5 @@
 #!/usr/bin/python
 
-import roslib; roslib.load_manifest('camera_pose_calibration')
 import tf2_ros
 import rospy
 import PyKDL

camera_pose_calibration/scripts/camera_info_converter.py

@@ -0,0 +1,13 @@
+#!/usr/bin/python
+
+from camera_pose_calibration.camera_info_converter import CameraInfoConverter
+
+def main():
+    rospy.init_node('camera_info_converter')
+    camera_converter = CameraInfoConverter()
+    rospy.spin()
+
+
+
+if __name__ == '__main__':
+    main()

camera_pose_calibration/src/camera_pose_calibration/capture_monitor.py → camera_pose_calibration/scripts/capture_monitor.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python
 
-import roslib; roslib.load_manifest('camera_pose_calibration')
 import cv
 from cv_bridge import CvBridge, CvBridgeError
 import rospy
@@ -185,22 +184,22 @@ def loop(self):
                 if self.capture_time+rospy.Duration(4.0) > rospy.Time.now():
                     if self.capture_time+rospy.Duration(2.0) > rospy.Time.now():
                         # Captured checkerboards
-                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="passthrough"))
-                    elif self.calibrate_time+rospy.Duration(8.0) > rospy.Time.now():
+                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="rgb8"))
+                    elif self.calibrate_time+rospy.Duration(20.0) > rospy.Time.now():
                         # Succeeded optimization
-                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="passthrough"))
+                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="rgb8"))
                         if beep_time+rospy.Duration(8.0) < rospy.Time.now():
                             beep_time = rospy.Time.now()
                             beep([(600, 63, 0.1), (800, 63, 0.1), (1000, 63, 0.3)])
                     else:
                         # Failed optimization
-                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="passthrough"))
+                        self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="rgb8"))
                         if beep_time+rospy.Duration(4.0) < rospy.Time.now():
                             beep_time = rospy.Time.now()
                             beep([(400, 63, 0.1), (200, 63, 0.1), (100, 63, 0.6)])
 
                 else:
-                    self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="passthrough"))
+                    self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="rgb8"))
 
 
 

camera_pose_calibration/src/camera_pose_calibration/filter_intervals.py → camera_pose_calibration/scripts/filter_intervals.py

@@ -2,7 +2,6 @@
 
 
 
-import roslib; roslib.load_manifest('camera_pose_calibration')
 import rospy
 import threading
 from calibration_msgs.msg import Interval, CalibrationPattern
@@ -30,8 +29,10 @@ def __init__(self):
 
     def interval_cb(self, msg):
         with self.lock:
+            print "got interval!"
             duration = msg.end - msg.start
             if self.feature and diff(self.feature, self.last_feature) > self.min_motion and duration > self.min_duration:
+                print "interval not good enough!"
                 self.last_feature = self.feature
                 self.pub.publish(msg)
 

camera_pose_calibration/src/camera_pose_calibration/flip_flop_kinect.py → camera_pose_calibration/scripts/flip_flop_kinect.py

@@ -1,6 +1,5 @@
 #!/usr/bin/python
 
-import roslib; roslib.load_manifest('camera_pose_calibration')
 import rospy
 from camera_pose_calibration.srv import TriggerOne, TriggerOneRequest