Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18528] pkg/trunk
|
From: <vij...@us...> - 2009-07-09 00:54:20
|
Revision: 18528
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18528&view=rev
Author: vijaypradeep
Date: 2009-07-09 00:54:16 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
StereoCheckerboard detector now publishes cb corners with a more reasonable msg type
Working on stereo checkerboard capture action
Modified Paths:
--------------
pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
Added Paths:
-----------
pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg
pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg
pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg
pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h
pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp
pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp
Added: pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbCamCorner.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,4 @@
+# Stores a single checkerboard corner, as sensed by a camera
+int32 x_index # The x index of the checkerboard corner
+int32 y_index # The y index of the checkerboard corner
+PixelPoint point # The sensed corner location in pixel coordinates
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbCamCorners.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,5 @@
+# Stores a set of checkerboard corners as sensed by a camera
+Header header
+CbCamCorner[] corners # Array of the sensed corners
+int16 num_x # The number of checkerboard corners in the x direction
+int16 num_y # The number of checkerboard corners in the y direction
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/CbStereoCorners.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+Header header
+CbCamCorners left
+CbCamCorners right
\ No newline at end of file
Added: pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg
===================================================================
--- pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg (rev 0)
+++ pkg/trunk/calibration/calibration_msgs/msg/PixelPoint.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+# Stores the xy location of a point in an image in pixel coordinates
+float64 x
+float64 y
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/CMakeLists.txt 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+rospack(pr2_calibration_actions)
+
+genmsg()
+
+rospack_add_library(${PROJECT_NAME} src/stereo_cb_action.cpp)
+rospack_add_boost_directories()
+rospack_link_boost(${PROJECT_NAME} thread)
+
+
+rospack_add_executable(run_action_stereo_cb src/run_action_stereo_cb.cpp)
+target_link_libraries(run_action_stereo_cb ${PROJECT_NAME})
+
+rospack_add_executable(test_stereo_cb src/test_stereo_cb.cpp)
+target_link_libraries(test_stereo_cb ${PROJECT_NAME})
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/Makefile 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/include/pr2_calibration_actions/stereo_cb_action.h 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,86 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef PR2_CALIBRATION_ACTIONS_STEREO_CB_ACTION_H_
+#define PR2_CALIBRATION_ACTIONS_STEREO_CB_ACTION_H_
+
+#include <string>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+
+#include "ros/ros.h"
+#include "robot_actions/action.h"
+
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+namespace pr2_calibration_actions
+{
+
+/**
+ *
+ */
+class StereoCbAction : public robot_actions::Action<CbCamCmd, calibration_msgs::CbStereoCorners>
+{
+public:
+ StereoCbAction();
+
+ virtual robot_actions::ResultStatus execute(const CbCamCmd& goal, calibration_msgs::CbStereoCorners& feedback) ;
+
+
+private:
+ void handlePreempt() ;
+ void cornersCallback(const calibration_msgs::CbStereoCornersConstPtr& msg) ;
+ void resetStereoAvg(const CbCamCmd& goal) ;
+ bool pixelTooFar(const calibration_msgs::PixelPoint& a, const calibration_msgs::PixelPoint& b, double tol) ;
+
+ ros::NodeHandle n_ ;
+ ros::Subscriber sub_ ;
+
+ std::string controller_;
+
+ boost::mutex data_mutex_ ;
+ boost::condition corners_available_ ;
+ calibration_msgs::CbStereoCorners avg_ ;
+ CbCamCmd goal_ ;
+ unsigned int num_samples_ ;
+ bool accumulating_ ;
+ bool need_to_abort_ ;
+
+} ;
+
+}
+
+
+#endif
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/manifest.xml 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,24 @@
+<package>
+ <description brief="pr2_calibration_actions">
+ Actions specific to the PR2 for collecting calibration information
+ </description>
+ <author>Vijay Pradeep / vpr...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/calibration_msgs</url>
+
+ <depend package="std_msgs" />
+ <depend package="robot_msgs" />
+ <depend package="sensor_msgs" />
+ <depend package="calibration_msgs" />
+ <depend package="pr2_msgs" />
+
+ <depend package="robot_actions" />
+
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp" />
+ </export>
+
+</package>
+
+
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/CbCamCmd.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,3 @@
+uint16 num_x # The number of checkerboard corners in the x direction
+uint16 num_y # The number of checkerboard corners in the y direction
+float64 pixel_tol # The max difference in sensed pixel location of a corner across a set of camera frames
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/msg/StereoCbActionState.msg 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,4 @@
+Header header
+CbCamCmd goal
+calibration_msgs/CbStereoCorners feedback
+robot_actions/ActionStatus status
\ No newline at end of file
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/run_action_stereo_cb.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,63 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "robot_actions/action_runner.h"
+
+#include "pr2_calibration_actions/stereo_cb_action.h"
+#include "pr2_calibration_actions/StereoCbActionState.h"
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+
+using namespace pr2_calibration_actions ;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "stereo_cb_runner") ;
+
+ ros::NodeHandle n ;
+
+ StereoCbAction action ;
+ robot_actions::ActionRunner runner(10.0) ;
+
+ runner.connect<CbCamCmd, StereoCbActionState, calibration_msgs::CbStereoCorners>(action) ;
+
+ ROS_DEBUG("Calling run") ;
+ runner.run() ;
+ ROS_DEBUG("Done Calling run") ;
+
+ ros::spin() ;
+
+ return 0 ;
+}
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/stereo_cb_action.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,178 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "pr2_calibration_actions/stereo_cb_action.h"
+#include "calibration_msgs/CbCamCorners.h"
+
+using namespace pr2_calibration_actions ;
+using namespace calibration_msgs ;
+
+StereoCbAction::StereoCbAction() : robot_actions::Action<CbCamCmd, CbStereoCorners>("stereo_cb_action")
+{
+ sub_ = n_.subscribe("stereo_corners", 1, &StereoCbAction::cornersCallback, this) ;
+ accumulating_ = false ;
+}
+
+robot_actions::ResultStatus StereoCbAction::execute(const CbCamCmd& goal, CbStereoCorners& feedback)
+{
+ boost::mutex::scoped_lock lock(data_mutex_) ;
+
+ resetStereoAvg(goal) ;
+ accumulating_ = true ;
+ need_to_abort_ = false ;
+ goal_ = goal ;
+
+ boost::posix_time::milliseconds condition_timeout(1000.0f) ; // 1 sec signal timeout
+ while(true)
+ {
+ corners_available_.timed_wait(data_mutex_, condition_timeout) ;
+
+ if (need_to_abort_)
+ {
+ ROS_DEBUG("ABORTING") ;
+ feedback = avg_ ;
+ return robot_actions::ABORTED ;
+ }
+
+ if (isPreemptRequested())
+ {
+ ROS_DEBUG("PREEMPTED") ;
+ feedback = avg_ ;
+ return robot_actions::PREEMPTED ;
+ }
+ }
+}
+
+void StereoCbAction::handlePreempt()
+{
+ ROS_DEBUG("Handle Preempt") ;
+ corners_available_.notify_all() ;
+}
+
+void StereoCbAction::cornersCallback(const calibration_msgs::CbStereoCornersConstPtr& msg)
+{
+ if (msg->left.corners.size() == 0)
+ {
+ ROS_INFO("Didn't see checkerboard in left cam") ;
+ return ;
+ }
+
+ if (msg->right.corners.size() == 0)
+ {
+ ROS_INFO("Didn't see checkerboard in right cam") ;
+ return ;
+ }
+
+ boost::mutex::scoped_lock lock(data_mutex_) ;
+
+ if (!accumulating_)
+ return ;
+
+ const unsigned int N = msg->left.corners.size() ;
+ if (msg->right.corners.size() != N)
+ {
+ ROS_ERROR("Left and right images don't have same # of corners") ;
+ need_to_abort_ = true ;
+ return ;
+ }
+
+ if (avg_.left.corners.size() != N)
+ {
+ ROS_ERROR("Received image does not have same # of corners as avg ") ;
+ need_to_abort_ = true ;
+ return ;
+ }
+
+ // Check pixel distances. Trigger an abort if the pixel dist is too far
+ for (unsigned int i=0; i<N; i++)
+ {
+ need_to_abort_ = need_to_abort_ || pixelTooFar(avg_.left.corners[i].point, msg->left.corners[i].point, goal_.pixel_tol) ;
+ need_to_abort_ = need_to_abort_ || pixelTooFar(avg_.right.corners[i].point, msg->right.corners[i].point, goal_.pixel_tol) ;
+ }
+
+ for (unsigned int i=0; i<N; i++)
+ {
+
+ if (avg_.left.corners[i].x_index != msg->left.corners[i].x_index ||
+ avg_.left.corners[i].y_index != msg->left.corners[i].y_index ||
+ avg_.right.corners[i].x_index != msg->right.corners[i].x_index ||
+ avg_.right.corners[i].y_index != msg->right.corners[i].y_index)
+ {
+ ROS_ERROR("Corner indicies don't line up. This action doesn't support this [tricky] case yet") ;
+ need_to_abort_ = true ;
+ }
+
+ avg_.left.corners[i].point.x = (avg_.left.corners[i].point.x*num_samples_ + msg->left.corners[i].point.x)/ (num_samples_+1) ;
+ avg_.left.corners[i].point.y = (avg_.left.corners[i].point.y*num_samples_ + msg->left.corners[i].point.y)/ (num_samples_+1) ;
+ avg_.right.corners[i].point.x = (avg_.right.corners[i].point.x*num_samples_ + msg->right.corners[i].point.x)/(num_samples_+1) ;
+ avg_.right.corners[i].point.y = (avg_.right.corners[i].point.y*num_samples_ + msg->right.corners[i].point.y)/(num_samples_+1) ;
+ }
+ num_samples_++ ;
+
+ corners_available_.notify_all() ;
+}
+
+void resetCbCamCorners(CbCamCorners& cam, const CbCamCmd& goal)
+{
+ cam.corners.resize(goal.num_x * goal.num_y) ;
+
+ for (unsigned int j=0; j<goal.num_y; j++)
+ {
+ for (unsigned int i=0; i<goal.num_x; i++)
+ {
+ cam.corners[j*goal.num_x + i].x_index = i ;
+ cam.corners[j*goal.num_x + i].y_index = j ;
+ cam.corners[j*goal.num_x + i].point.x = 0.0 ;
+ cam.corners[j*goal.num_x + i].point.y = 0.0 ;
+ }
+ }
+
+ cam.num_x = goal.num_x ;
+ cam.num_y = goal.num_y ;
+}
+
+void StereoCbAction::resetStereoAvg(const CbCamCmd& goal)
+{
+ resetCbCamCorners(avg_.left, goal) ;
+ resetCbCamCorners(avg_.right, goal) ;
+}
+
+
+bool StereoCbAction::pixelTooFar(const PixelPoint& a, const PixelPoint& b, double tol)
+{
+ double dx = a.x - b.x ;
+ double dy = a.y - b.y ;
+
+ return dx > tol || dx < -tol || dy > tol || dy < -tol ;
+}
Added: pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp (rev 0)
+++ pkg/trunk/calibration/sandbox/pr2_calibration_actions/src/test_stereo_cb.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include "robot_actions/action_client.h"
+
+#include "pr2_calibration_actions/StereoCbActionState.h"
+#include "pr2_calibration_actions/CbCamCmd.h"
+#include "calibration_msgs/CbStereoCorners.h"
+
+
+using namespace pr2_calibration_actions ;
+
+void spinThread()
+{
+ ros::spin();
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "test_single_scan") ;
+
+ ros::NodeHandle n ;
+
+ boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+
+ robot_actions::ActionClient<CbCamCmd, StereoCbActionState, calibration_msgs::CbStereoCorners> client("stereo_cb_action") ;
+ ros::Duration duration ;
+
+ duration.fromSec(3.0) ;
+ ROS_DEBUG("Sleeping") ;
+ duration.sleep() ;
+ ROS_DEBUG("Done Sleeping") ;
+
+ CbCamCmd goal ;
+ goal.num_x = 6 ;
+ goal.num_y = 8 ;
+ goal.pixel_tol = 2 ;
+
+ calibration_msgs::CbStereoCorners feedback ;
+
+ robot_actions::ResultStatus result = client.execute(goal, feedback, ros::Duration().fromSec(30.0)) ;
+
+ ROS_DEBUG("ResultStatus=%u", result) ;
+
+ switch(result)
+ {
+ case robot_actions::SUCCESS :
+ ROS_DEBUG("SUCCESS!") ;
+ break ;
+ case robot_actions::ABORTED :
+ ROS_DEBUG("aborted") ;
+ break ;
+ case robot_actions::PREEMPTED :
+ ROS_DEBUG("preempted") ;
+ break ;
+ default :
+ ROS_DEBUG("other") ;
+ break ;
+ }
+
+ ROS_DEBUG("CB Size: [%u, %u]", feedback.left.num_x, feedback.right.num_y) ;
+
+ return 0 ;
+}
Modified: pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml 2009-07-09 00:53:46 UTC (rev 18527)
+++ pkg/trunk/vision/stereo_checkerboard_detector/manifest.xml 2009-07-09 00:54:16 UTC (rev 18528)
@@ -9,6 +9,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
+ <depend package="calibration_msgs"/>
<depend package="opencv_latest"/>
<depend package="sensor_msgs"/>
<depend package="topic_synchronizer"/>
Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-07-09 00:53:46 UTC (rev 18527)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-07-09 00:54:16 UTC (rev 18528)
@@ -43,9 +43,12 @@
#include "topic_synchronizer/topic_synchronizer.h"
#include "visualization_msgs/Marker.h"
+#include "calibration_msgs/CbStereoCorners.h"
+#include "calibration_msgs/CbCamCorners.h"
using namespace stereo_checkerboard_detector ;
using namespace std ;
+using namespace calibration_msgs ;
class StereoCheckerboardNode
{
@@ -56,13 +59,16 @@
ros::Duration().fromSec(0.05),
&StereoCheckerboardNode::msgTimeout )
{
- cb_helper_.setSize(6,8, .107) ;
+ num_x_ = 6 ;
+ num_y_ = 8 ;
+ cb_helper_.setSize(num_x_, num_y_, .107) ;
sync_.subscribe("stereo/left/image_rect", left_image_msg_, 1) ;
sync_.subscribe("stereo/left/cam_info", left_info_msg_, 1) ;
sync_.subscribe("stereo/right/image_rect",right_image_msg_, 1) ;
sync_.subscribe("stereo/right/cam_info", right_info_msg_, 1) ;
+ node_->advertise<calibration_msgs::CbStereoCorners>("cb_stereo_corners", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners_left", 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners_right", 1) ;
@@ -90,6 +96,30 @@
}
}
+ void pointVecToCbCamCorners(const vector<robot_msgs::Point>& vec, unsigned int num_x, unsigned int num_y,
+ calibration_msgs::CbCamCorners& corners)
+ {
+ corners.num_x = num_x ;
+ corners.num_y = num_y ;
+ if (vec.size() != num_x * num_y)
+ {
+ ROS_ERROR("vec.size()=%u, num_x=%u, num_y=%u", vec.size(), num_x, num_y) ;
+ return ;
+ }
+
+ for (unsigned int j=0; j<num_y; j++)
+ {
+ for (unsigned int i=0; i<num_x; i++)
+ {
+ const unsigned int ind = j*num_x + num_x ;
+ corners.corners[ind].x_index = i ;
+ corners.corners[ind].y_index = j ;
+ corners.corners[ind].point.x = vec[ind].x ;
+ corners.corners[ind].point.y = vec[ind].y ;
+ }
+ }
+ }
+
void msgCallback(ros::Time t)
{
bool found = cb_helper_.findCheckerboard(left_image_msg_, right_image_msg_, left_info_msg_, right_info_msg_) ;
@@ -97,6 +127,13 @@
robot_msgs::PointCloud cloud ;
cloud.header.frame_id = left_image_msg_.header.frame_id ;
cloud.header.stamp = left_image_msg_.header.stamp ;
+
+ CbStereoCorners stereo_corners ;
+ stereo_corners.header.frame_id = left_image_msg_.header.frame_id ;
+ stereo_corners.header.stamp = left_image_msg_.header.stamp ;
+ stereo_corners.left.header = stereo_corners.header ;
+ stereo_corners.right.header = stereo_corners.header ;
+
if (found)
{
//unsigned int N = xyz.size() ;
@@ -109,15 +146,21 @@
node_->publish("cb_corners", cloud) ;
// Publish the left corners
- cb_helper_.getCornersLeft(point_vec) ;
- pointVecToCloud(point_vec, cloud) ;
+ vector<robot_msgs::Point> left_point_vec ;
+ cb_helper_.getCornersLeft(left_point_vec) ;
+ pointVecToCloud(left_point_vec, cloud) ;
+ pointVecToCbCamCorners(left_point_vec, num_x_, num_y_, stereo_corners.left) ;
node_->publish("cb_corners_left", cloud) ;
// Publish the right corners
- cb_helper_.getCornersRight(point_vec) ;
- pointVecToCloud(point_vec, cloud) ;
+ vector<robot_msgs::Point> right_point_vec ;
+ cb_helper_.getCornersRight(right_point_vec) ;
+ pointVecToCloud(right_point_vec, cloud) ;
+ pointVecToCbCamCorners(right_point_vec, num_x_, num_y_, stereo_corners.right) ;
node_->publish("cb_corners_right", cloud) ;
+ node_->publish("cb_stereo_corners", stereo_corners) ;
+
// Publish a mini-axes defining the checkerboard
tf::Pose pose ;
cb_helper_.getPose(pose) ;
@@ -136,13 +179,20 @@
cloud.pts.clear() ;
node_->publish("cb_corners_left", cloud) ;
node_->publish("cb_corners_right", cloud) ;
+
+ stereo_corners.left.num_x = num_x_ ;
+ stereo_corners.left.num_y = num_y_ ;
+ stereo_corners.left.corners.clear() ;
+ stereo_corners.right.num_x = num_x_ ;
+ stereo_corners.right.num_y = num_y_ ;
+ stereo_corners.left.corners.clear() ;
+ node_->publish("cb_stereo_corners", stereo_corners) ;
printf("Not found\n") ;
}
node_->publish("left_cb_debug", cb_helper_.getLeftDebug()) ;
node_->publish("right_cb_debug", cb_helper_.getRightDebug()) ;
-
}
void msgTimeout(ros::Time t)
@@ -217,6 +267,9 @@
StereoCheckerboardHelper cb_helper_ ;
TopicSynchronizer<StereoCheckerboardNode> sync_ ;
+ unsigned int num_x_ ;
+ unsigned int num_y_ ;
+
sensor_msgs::Image left_image_msg_ ;
sensor_msgs::CamInfo left_info_msg_ ;
sensor_msgs::Image right_image_msg_ ;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
