Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23152] pkg/trunk
|
From: <vij...@us...> - 2009-08-27 17:21:32
|
Revision: 23152
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23152&view=rev
Author: vijaypradeep
Date: 2009-08-27 17:21:19 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Moving camera_offsetter out of experimental
Added Paths:
-----------
pkg/trunk/stacks/calibration/camera_offsetter/
pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt
pkg/trunk/stacks/calibration/camera_offsetter/Makefile
pkg/trunk/stacks/calibration/camera_offsetter/config/
pkg/trunk/stacks/calibration/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
pkg/trunk/stacks/calibration/camera_offsetter/config/prg.narrow_stereo.offset
pkg/trunk/stacks/calibration/camera_offsetter/include/
pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/
pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/generic_offsetter.h
pkg/trunk/stacks/calibration/camera_offsetter/launch/
pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter.launch
pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
pkg/trunk/stacks/calibration/camera_offsetter/manifest.xml
pkg/trunk/stacks/calibration/camera_offsetter/src/
pkg/trunk/stacks/calibration/camera_offsetter/src/keyboard_twist_generator.cpp
pkg/trunk/stacks/calibration/camera_offsetter/src/mono_offsetter.cpp
pkg/trunk/stacks/calibration/camera_offsetter/src/stereo_offsetter.cpp
pkg/trunk/stacks/calibration/camera_offsetter/test/
pkg/trunk/stacks/calibration/camera_offsetter/test/test_stereo_offsetter.launch
Removed Paths:
-------------
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp
pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,32 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rospack(camera_offsetter)
-
-#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)
-
-#uncomment if you have defined messages
-#genmsg()
-#uncomment if you have defined services
-#gensrv()
-
-#common commands for building c++ executables and libraries
-#rospack_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rospack_add_boost_directories()
-#rospack_link_boost(${PROJECT_NAME} thread)
-rospack_add_executable(mono_offsetter src/mono_offsetter.cpp)
-rospack_add_executable(stereo_offsetter src/stereo_offsetter.cpp)
-rospack_add_executable(keyboard_twist_generator src/keyboard_twist_generator.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,2 +0,0 @@
-0.024 0 0.244
-4.56455e-05 -0.00872199 0.00523311 0.999948
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,2 +0,0 @@
--0.000235494 -0.000141299 0.0269986
-0.00261665 -0.00523329 9.12921e-06 0.999983
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,183 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#ifndef CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
-#define CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
-
-#include <ros/ros.h>
-
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
-
-#include <geometry_msgs/Pose.h>
-#include <geometry_msgs/Twist.h>
-
-#include <boost/thread.hpp>
-#include <boost/thread/mutex.hpp>
-
-#include <fstream>
-
-namespace camera_offsetter
-{
-
-class GenericOffsetter
-{
-public:
- GenericOffsetter() : virtual_offset_( btQuaternion(0,0,0,1))
- {
- pose_sub_ = nh_.subscribe("~virtual_pose", 1, &GenericOffsetter::poseCb, this);
- twist_sub_ = nh_.subscribe("~virtual_twist", 1, &GenericOffsetter::twistCb, this);
-
- nh_.param("~position_scaling", position_scaling_, 1.0);
- nh_.param("~angular_scaling", angular_scaling_, 1.0);
-
- nh_.param("~config_file", config_file_, std::string("N/A"));
-
- nh_.param("~frame_suffix", frame_suffix_, std::string("_offset"));
-
- readConfig();
- }
-
- void readConfig()
- {
- if(config_file_==std::string("N/A"))
- {
- ROS_WARN("No config file is set");
- return;
- }
-
- geometry_msgs::PosePtr p(new geometry_msgs::Pose());
- std::fstream f (config_file_.c_str(), std::fstream::in);
- if(!f.is_open())
- {
- ROS_WARN("Couldn't open config file");
- return;
- }
- f >> p->position.x;
- f >> p->position.y;
- f >> p->position.z;
- f >> p->orientation.x;
- f >> p->orientation.y;
- f >> p->orientation.z;
- f >> p->orientation.w;
- ROS_INFO_STREAM(p->orientation.x);
- ROS_INFO_STREAM(p);
- poseCb(p);
- }
- void saveConfig()
- {
- if(config_file_==std::string("N/A"))
- {
- ROS_WARN("No config file is set");
- return;
- }
- std::fstream f (config_file_.c_str(), std::fstream::out);
- f << virtual_offset_.getOrigin().x() << " ";
- f << virtual_offset_.getOrigin().y() << " ";
- f << virtual_offset_.getOrigin().z() << std::endl;
- f << virtual_offset_.getRotation().x()<< " ";
- f << virtual_offset_.getRotation().y()<< " ";
- f << virtual_offset_.getRotation().z()<< " ";
- f << virtual_offset_.getRotation().w();
- }
-
- void poseCb(const geometry_msgs::PoseConstPtr& msg)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
- tf::poseMsgToTF(*msg, virtual_offset_);
- }
-
- void twistCb(const geometry_msgs::TwistConstPtr& msg)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
-
- btVector3 rot_vector(msg->angular.x, msg->angular.y, msg->angular.z);
- double angle = rot_vector.length();
-
- btQuaternion rot;
- if (angle < 1e-6)
- {
- rot = btQuaternion(0,0,0,1);
- }
- else
- rot = btQuaternion( rot_vector, angle);
-
- btVector3 trans( msg->linear.x, msg->linear.y, msg->linear.z);
-
- btTransform incrementalT(rot, trans);
-
- virtual_offset_ = virtual_offset_ * incrementalT;
-
- saveConfig();
- }
-
- void publishTransform(const ros::Time& time, const std::string frame_id, const std::string parent_id)
- {
- boost::mutex::scoped_lock lock(offset_mutex_);
- printf("Sending Transform: Trans:(%.3f, %.3f, %.3f) Q:(%.3f, %.3f , %.3f, %.3f)\n",
- virtual_offset_.getOrigin().x(),
- virtual_offset_.getOrigin().y(),
- virtual_offset_.getOrigin().z(),
- virtual_offset_.getRotation().x(),
- virtual_offset_.getRotation().y(),
- virtual_offset_.getRotation().z(),
- virtual_offset_.getRotation().w());
- tf_.sendTransform(virtual_offset_, time, frame_id, parent_id);
- }
-protected:
- std::string frame_suffix_;
-
-private:
- ros::NodeHandle nh_;
-
- ros::Subscriber pose_sub_;
- ros::Subscriber twist_sub_;
-
- tf::TransformBroadcaster tf_;
-
- double position_scaling_;
- double angular_scaling_;
-
- std::string config_file_;
-
- boost::mutex offset_mutex_;
- btTransform virtual_offset_;
-} ;
-
-
-}
-
-#endif
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,14 +0,0 @@
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo.offset" />
-
- </node>
-
-</launch>
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,16 +0,0 @@
-
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo_to_laser.offset" />
- <param name="frame_suffix" type="string" value="_offset_laser" />
-
- </node>
-
-</launch>
\ No newline at end of file
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,22 +0,0 @@
-<package>
- <description brief="Hand tweak camera extrinsics">
- Provides a tool to make small changes to the location of a monocular camera or stereocamera attached to the robot
- </description>
- <author>Vijay Pradeep, Alex Sorokin</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <url>http://pr.willowgarage.com/wiki/camera_offsetter</url>
-
- <!-- ROS -->
- <depend package="roscpp" />
-
- <!-- geometry -->
- <depend package="tf" />
-
- <!-- common_msgs -->
- <depend package="geometry_msgs" />
- <depend package="sensor_msgs" />
-
- <!-- stereo -->
- <depend package="stereo_msgs" />
-</package>
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,131 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include <ros/ros.h>
-#include <geometry_msgs/Twist.h>
-
-#include <unistd.h>
-#include <termios.h>
-
-int main(int argc, char** argv)
-{
- // Setup terminal settings for getchar
- const int fd = fileno(stdin);
- termios prev_flags ;
- tcgetattr(fd, &prev_flags) ;
- termios flags ;
- tcgetattr(fd,&flags);
- flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
- flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
- flags.c_cc[VTIME] = 0; // block if waiting for char
- tcsetattr(fd,TCSANOW,&flags);
-
- ros::init(argc, argv, "keyboard_twist_generator");
-
- ros::NodeHandle nh;
-
- ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("keyboard_twist", 10);
-
- static const double trans = .001;
- static const double rot = .1 * 3.14 / 180.0;
-
-
- enum ShiftMode { ROTATION=0, TRANSLATION=1 };
-
- ShiftMode cur_shift_mode = TRANSLATION;
-
- while (nh.ok())
- {
- char c = getchar();
-
- geometry_msgs::Twist twist;
-
- geometry_msgs::Vector3* vec = NULL;
- double shift_val;
- switch (cur_shift_mode)
- {
- case ROTATION:
- vec = &twist.angular;
- shift_val = rot;
- break;
- case TRANSLATION:
- vec = &twist.linear;
- shift_val = trans;
- break;
- default:
- shift_val = 0.0;
- ROS_FATAL("unknown shift type [%u]", cur_shift_mode);
- break;
- }
-
- bool should_publish = false;
-
- switch (c)
- {
- case 'x': vec->x = -shift_val; should_publish = true; break;
- case 'X': vec->x = shift_val; should_publish = true; break;
- case 'y': vec->y = -shift_val; should_publish = true; break;
- case 'Y': vec->y = shift_val; should_publish = true; break;
- case 'z': vec->z = -shift_val; should_publish = true; break;
- case 'Z': vec->z = shift_val; should_publish = true; break;
- case 't':
- case 'T':
- ROS_INFO("Switching to translation mode");
- cur_shift_mode = TRANSLATION;
- break;
- case 'r':
- case 'R':
- ROS_INFO("Switching to rotation mode");
- cur_shift_mode = ROTATION;
- break;
- default:
- usleep(100);
- break;
- }
-
- if (should_publish)
- {
- ROS_INFO("Publishing Twist: Translation(%.3f, %.3f, %.3f) Rotation(%.3f, %.3f, %.3f)",
- twist.linear.x, twist.linear.y, twist.linear.z,
- twist.angular.x, twist.angular.y, twist.angular.z);
- pub.publish(twist);
- }
- }
-
- tcsetattr(fd,TCSANOW, &prev_flags) ; // Undo any terminal changes that we made
-
- return 0;
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,98 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include "camera_offsetter/generic_offsetter.h"
-#include "sensor_msgs/Image.h"
-#include "sensor_msgs/CameraInfo.h"
-
-namespace camera_offsetter
-{
-
-class MonoOffsetter : public GenericOffsetter
-{
-public:
- MonoOffsetter()
- {
- if (!nh_.getParam("~cam_name", cam_name_))
- ROS_FATAL("Couldn't find param [~cam_name]");
-
- image_sub_ = nh_.subscribe(cam_name_ + "/image_rect", 1, &MonoOffsetter::imageCb, this);
- info_sub_ = nh_.subscribe(cam_name_ + "/cam_info", 1, &MonoOffsetter::infoCb, this);
-
- image_pub_ = nh_.advertise<sensor_msgs::Image>(cam_name_ + "_offset/image_rect", 1);
- info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(cam_name_ + "_offset/cam_info", 1);
- }
-
- void imageCb(const sensor_msgs::ImageConstPtr& msg)
- {
- sensor_msgs::Image next_image = *msg;
- next_image.header.frame_id = msg->header.frame_id + frame_suffix_;
- image_pub_.publish(next_image);
-
- publishTransform(msg->header.stamp, next_image.header.frame_id, msg->header.frame_id);
- }
-
- void infoCb(const sensor_msgs::CameraInfoConstPtr& msg)
- {
- sensor_msgs::CameraInfo next_info = *msg;
- next_info.header.frame_id = msg->header.frame_id + frame_suffix_;
- info_pub_.publish(next_info);
- }
-
-private:
- ros::NodeHandle nh_;
- ros::Subscriber image_sub_;
- ros::Subscriber info_sub_;
-
- ros::Publisher image_pub_;
- ros::Publisher info_pub_;
-
-
- std::string cam_name_;
-} ;
-
-}
-
-using namespace camera_offsetter;
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "mono_offsetter");
-
- MonoOffsetter offsetter;
-
- ros::spin();
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,84 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, 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.
- *********************************************************************/
-
-//! \author Vijay Pradeep
-
-#include "camera_offsetter/generic_offsetter.h"
-#include "stereo_msgs/RawStereo.h"
-
-namespace camera_offsetter
-{
-
-class StereoOffsetter : public GenericOffsetter
-{
-public:
- StereoOffsetter()
- {
- if (!nh_.getParam("~cam_name", cam_name_))
- ROS_FATAL("Couldn't find param [~cam_name]");
-
- raw_stereo_sub_ = nh_.subscribe(cam_name_ + "/raw_stereo", 1, &StereoOffsetter::rawStereoCb, this);
-
- raw_stereo_pub_ = nh_.advertise<stereo_msgs::RawStereo>(cam_name_ + "_offset/raw_stereo", 1);
- }
-
- void rawStereoCb(const stereo_msgs::RawStereoConstPtr& msg)
- {
- stereo_msgs::RawStereo next_raw_stereo = *msg;
- next_raw_stereo.header.frame_id = msg->header.frame_id + frame_suffix_;
- raw_stereo_pub_.publish(next_raw_stereo);
-
- publishTransform(msg->header.stamp, next_raw_stereo.header.frame_id, msg->header.frame_id);
- }
-
-private:
- ros::NodeHandle nh_;
- ros::Subscriber raw_stereo_sub_;
- ros::Publisher raw_stereo_pub_;
-
- std::string cam_name_;
-} ;
-
-}
-
-using namespace camera_offsetter;
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "stereo_offsetter");
-
- StereoOffsetter offsetter;
-
- ros::spin();
-}
Deleted: pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch
===================================================================
--- pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-27 17:20:32 UTC (rev 23151)
+++ pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -1,11 +0,0 @@
-<launch>
-
- <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
- <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
- <remap from="~virtual_twist" to="keyboard_twist" />
- <param name="cam_name" type="string" value="/narrow_stereo" />
- <param name="position_scaling" type="double" value=".0001" />
- <param name="angular_scaling" type="double" value="1.0" />
- </node>
-
-</launch>
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/CMakeLists.txt)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/CMakeLists.txt 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+# Set the build type. Options are:
+# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
+# Debug : w/ debug symbols, w/o optimization
+# Release : w/o debug symbols, w/ optimization
+# RelWithDebInfo : w/ debug symbols, w/ optimization
+# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rospack(camera_offsetter)
+
+#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)
+
+#uncomment if you have defined messages
+#genmsg()
+#uncomment if you have defined services
+#gensrv()
+
+#common commands for building c++ executables and libraries
+#rospack_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rospack_add_boost_directories()
+#rospack_link_boost(${PROJECT_NAME} thread)
+rospack_add_executable(mono_offsetter src/mono_offsetter.cpp)
+rospack_add_executable(stereo_offsetter src/stereo_offsetter.cpp)
+rospack_add_executable(keyboard_twist_generator src/keyboard_twist_generator.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Copied: pkg/trunk/stacks/calibration/camera_offsetter/Makefile (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/Makefile)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/Makefile (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/Makefile 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/config/hcb.narrow_stereo_to_laser.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,2 @@
+0.024 0 0.244
+4.56455e-05 -0.00872199 0.00523311 0.999948
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/config/prg.narrow_stereo.offset (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/config/prg.narrow_stereo.offset)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/config/prg.narrow_stereo.offset (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/config/prg.narrow_stereo.offset 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,2 @@
+-0.000235494 -0.000141299 0.0269986
+0.00261665 -0.00523329 9.12921e-06 0.999983
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/generic_offsetter.h (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/include/camera_offsetter/generic_offsetter.h)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/generic_offsetter.h (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/include/camera_offsetter/generic_offsetter.h 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,183 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#ifndef CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
+#define CAMERA_OFFSETTER_GENERIC_OFFSETTER_H_
+
+#include <ros/ros.h>
+
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Twist.h>
+
+#include <boost/thread.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <fstream>
+
+namespace camera_offsetter
+{
+
+class GenericOffsetter
+{
+public:
+ GenericOffsetter() : virtual_offset_( btQuaternion(0,0,0,1))
+ {
+ pose_sub_ = nh_.subscribe("~virtual_pose", 1, &GenericOffsetter::poseCb, this);
+ twist_sub_ = nh_.subscribe("~virtual_twist", 1, &GenericOffsetter::twistCb, this);
+
+ nh_.param("~position_scaling", position_scaling_, 1.0);
+ nh_.param("~angular_scaling", angular_scaling_, 1.0);
+
+ nh_.param("~config_file", config_file_, std::string("N/A"));
+
+ nh_.param("~frame_suffix", frame_suffix_, std::string("_offset"));
+
+ readConfig();
+ }
+
+ void readConfig()
+ {
+ if(config_file_==std::string("N/A"))
+ {
+ ROS_WARN("No config file is set");
+ return;
+ }
+
+ geometry_msgs::PosePtr p(new geometry_msgs::Pose());
+ std::fstream f (config_file_.c_str(), std::fstream::in);
+ if(!f.is_open())
+ {
+ ROS_WARN("Couldn't open config file");
+ return;
+ }
+ f >> p->position.x;
+ f >> p->position.y;
+ f >> p->position.z;
+ f >> p->orientation.x;
+ f >> p->orientation.y;
+ f >> p->orientation.z;
+ f >> p->orientation.w;
+ ROS_INFO_STREAM(p->orientation.x);
+ ROS_INFO_STREAM(p);
+ poseCb(p);
+ }
+ void saveConfig()
+ {
+ if(config_file_==std::string("N/A"))
+ {
+ ROS_WARN("No config file is set");
+ return;
+ }
+ std::fstream f (config_file_.c_str(), std::fstream::out);
+ f << virtual_offset_.getOrigin().x() << " ";
+ f << virtual_offset_.getOrigin().y() << " ";
+ f << virtual_offset_.getOrigin().z() << std::endl;
+ f << virtual_offset_.getRotation().x()<< " ";
+ f << virtual_offset_.getRotation().y()<< " ";
+ f << virtual_offset_.getRotation().z()<< " ";
+ f << virtual_offset_.getRotation().w();
+ }
+
+ void poseCb(const geometry_msgs::PoseConstPtr& msg)
+ {
+ boost::mutex::scoped_lock lock(offset_mutex_);
+ tf::poseMsgToTF(*msg, virtual_offset_);
+ }
+
+ void twistCb(const geometry_msgs::TwistConstPtr& msg)
+ {
+ boost::mutex::scoped_lock lock(offset_mutex_);
+
+ btVector3 rot_vector(msg->angular.x, msg->angular.y, msg->angular.z);
+ double angle = rot_vector.length();
+
+ btQuaternion rot;
+ if (angle < 1e-6)
+ {
+ rot = btQuaternion(0,0,0,1);
+ }
+ else
+ rot = btQuaternion( rot_vector, angle);
+
+ btVector3 trans( msg->linear.x, msg->linear.y, msg->linear.z);
+
+ btTransform incrementalT(rot, trans);
+
+ virtual_offset_ = virtual_offset_ * incrementalT;
+
+ saveConfig();
+ }
+
+ void publishTransform(const ros::Time& time, const std::string frame_id, const std::string parent_id)
+ {
+ boost::mutex::scoped_lock lock(offset_mutex_);
+ printf("Sending Transform: Trans:(%.3f, %.3f, %.3f) Q:(%.3f, %.3f , %.3f, %.3f)\n",
+ virtual_offset_.getOrigin().x(),
+ virtual_offset_.getOrigin().y(),
+ virtual_offset_.getOrigin().z(),
+ virtual_offset_.getRotation().x(),
+ virtual_offset_.getRotation().y(),
+ virtual_offset_.getRotation().z(),
+ virtual_offset_.getRotation().w());
+ tf_.sendTransform(virtual_offset_, time, frame_id, parent_id);
+ }
+protected:
+ std::string frame_suffix_;
+
+private:
+ ros::NodeHandle nh_;
+
+ ros::Subscriber pose_sub_;
+ ros::Subscriber twist_sub_;
+
+ tf::TransformBroadcaster tf_;
+
+ double position_scaling_;
+ double angular_scaling_;
+
+ std::string config_file_;
+
+ boost::mutex offset_mutex_;
+ btTransform virtual_offset_;
+} ;
+
+
+}
+
+#endif
Copied: pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter.launch (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter.launch)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter.launch (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,14 @@
+<launch>
+
+ <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
+ <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
+ <remap from="~virtual_twist" to="keyboard_twist" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="position_scaling" type="double" value=".0001" />
+ <param name="angular_scaling" type="double" value="1.0" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo.offset" />
+
+ </node>
+
+</launch>
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/launch/narrow_stereo_offsetter_to_laser.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,16 @@
+
+<launch>
+
+ <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
+ <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
+ <remap from="~virtual_twist" to="keyboard_twist" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="position_scaling" type="double" value=".0001" />
+ <param name="angular_scaling" type="double" value="1.0" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="config_file" type="string" value="$(find camera_offsetter)/config/$(env ROBOT).narrow_stereo_to_laser.offset" />
+ <param name="frame_suffix" type="string" value="_offset_laser" />
+
+ </node>
+
+</launch>
\ No newline at end of file
Copied: pkg/trunk/stacks/calibration/camera_offsetter/manifest.xml (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/manifest.xml)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/manifest.xml (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/manifest.xml 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,22 @@
+<package>
+ <description brief="Hand tweak camera extrinsics">
+ Provides a tool to make small changes to the location of a monocular camera or stereocamera attached to the robot
+ </description>
+ <author>Vijay Pradeep, Alex Sorokin</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/camera_offsetter</url>
+
+ <!-- ROS -->
+ <depend package="roscpp" />
+
+ <!-- geometry -->
+ <depend package="tf" />
+
+ <!-- common_msgs -->
+ <depend package="geometry_msgs" />
+ <depend package="sensor_msgs" />
+
+ <!-- stereo -->
+ <depend package="stereo_msgs" />
+</package>
Copied: pkg/trunk/stacks/calibration/camera_offsetter/src/keyboard_twist_generator.cpp (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/keyboard_twist_generator.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/src/keyboard_twist_generator.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/src/keyboard_twist_generator.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,131 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+
+#include <unistd.h>
+#include <termios.h>
+
+int main(int argc, char** argv)
+{
+ // Setup terminal settings for getchar
+ const int fd = fileno(stdin);
+ termios prev_flags ;
+ tcgetattr(fd, &prev_flags) ;
+ termios flags ;
+ tcgetattr(fd,&flags);
+ flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
+ flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
+ flags.c_cc[VTIME] = 0; // block if waiting for char
+ tcsetattr(fd,TCSANOW,&flags);
+
+ ros::init(argc, argv, "keyboard_twist_generator");
+
+ ros::NodeHandle nh;
+
+ ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("keyboard_twist", 10);
+
+ static const double trans = .001;
+ static const double rot = .1 * 3.14 / 180.0;
+
+
+ enum ShiftMode { ROTATION=0, TRANSLATION=1 };
+
+ ShiftMode cur_shift_mode = TRANSLATION;
+
+ while (nh.ok())
+ {
+ char c = getchar();
+
+ geometry_msgs::Twist twist;
+
+ geometry_msgs::Vector3* vec = NULL;
+ double shift_val;
+ switch (cur_shift_mode)
+ {
+ case ROTATION:
+ vec = &twist.angular;
+ shift_val = rot;
+ break;
+ case TRANSLATION:
+ vec = &twist.linear;
+ shift_val = trans;
+ break;
+ default:
+ shift_val = 0.0;
+ ROS_FATAL("unknown shift type [%u]", cur_shift_mode);
+ break;
+ }
+
+ bool should_publish = false;
+
+ switch (c)
+ {
+ case 'x': vec->x = -shift_val; should_publish = true; break;
+ case 'X': vec->x = shift_val; should_publish = true; break;
+ case 'y': vec->y = -shift_val; should_publish = true; break;
+ case 'Y': vec->y = shift_val; should_publish = true; break;
+ case 'z': vec->z = -shift_val; should_publish = true; break;
+ case 'Z': vec->z = shift_val; should_publish = true; break;
+ case 't':
+ case 'T':
+ ROS_INFO("Switching to translation mode");
+ cur_shift_mode = TRANSLATION;
+ break;
+ case 'r':
+ case 'R':
+ ROS_INFO("Switching to rotation mode");
+ cur_shift_mode = ROTATION;
+ break;
+ default:
+ usleep(100);
+ break;
+ }
+
+ if (should_publish)
+ {
+ ROS_INFO("Publishing Twist: Translation(%.3f, %.3f, %.3f) Rotation(%.3f, %.3f, %.3f)",
+ twist.linear.x, twist.linear.y, twist.linear.z,
+ twist.angular.x, twist.angular.y, twist.angular.z);
+ pub.publish(twist);
+ }
+ }
+
+ tcsetattr(fd,TCSANOW, &prev_flags) ; // Undo any terminal changes that we made
+
+ return 0;
+}
Copied: pkg/trunk/stacks/calibration/camera_offsetter/src/mono_offsetter.cpp (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/mono_offsetter.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/src/mono_offsetter.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/src/mono_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,98 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include "camera_offsetter/generic_offsetter.h"
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/CameraInfo.h"
+
+namespace camera_offsetter
+{
+
+class MonoOffsetter : public GenericOffsetter
+{
+public:
+ MonoOffsetter()
+ {
+ if (!nh_.getParam("~cam_name", cam_name_))
+ ROS_FATAL("Couldn't find param [~cam_name]");
+
+ image_sub_ = nh_.subscribe(cam_name_ + "/image_rect", 1, &MonoOffsetter::imageCb, this);
+ info_sub_ = nh_.subscribe(cam_name_ + "/cam_info", 1, &MonoOffsetter::infoCb, this);
+
+ image_pub_ = nh_.advertise<sensor_msgs::Image>(cam_name_ + "_offset/image_rect", 1);
+ info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(cam_name_ + "_offset/cam_info", 1);
+ }
+
+ void imageCb(const sensor_msgs::ImageConstPtr& msg)
+ {
+ sensor_msgs::Image next_image = *msg;
+ next_image.header.frame_id = msg->header.frame_id + frame_suffix_;
+ image_pub_.publish(next_image);
+
+ publishTransform(msg->header.stamp, next_image.header.frame_id, msg->header.frame_id);
+ }
+
+ void infoCb(const sensor_msgs::CameraInfoConstPtr& msg)
+ {
+ sensor_msgs::CameraInfo next_info = *msg;
+ next_info.header.frame_id = msg->header.frame_id + frame_suffix_;
+ info_pub_.publish(next_info);
+ }
+
+private:
+ ros::NodeHandle nh_;
+ ros::Subscriber image_sub_;
+ ros::Subscriber info_sub_;
+
+ ros::Publisher image_pub_;
+ ros::Publisher info_pub_;
+
+
+ std::string cam_name_;
+} ;
+
+}
+
+using namespace camera_offsetter;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "mono_offsetter");
+
+ MonoOffsetter offsetter;
+
+ ros::spin();
+}
Copied: pkg/trunk/stacks/calibration/camera_offsetter/src/stereo_offsetter.cpp (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/src/stereo_offsetter.cpp)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/src/stereo_offsetter.cpp (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/src/stereo_offsetter.cpp 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,84 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, 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.
+ *********************************************************************/
+
+//! \author Vijay Pradeep
+
+#include "camera_offsetter/generic_offsetter.h"
+#include "stereo_msgs/RawStereo.h"
+
+namespace camera_offsetter
+{
+
+class StereoOffsetter : public GenericOffsetter
+{
+public:
+ StereoOffsetter()
+ {
+ if (!nh_.getParam("~cam_name", cam_name_))
+ ROS_FATAL("Couldn't find param [~cam_name]");
+
+ raw_stereo_sub_ = nh_.subscribe(cam_name_ + "/raw_stereo", 1, &StereoOffsetter::rawStereoCb, this);
+
+ raw_stereo_pub_ = nh_.advertise<stereo_msgs::RawStereo>(cam_name_ + "_offset/raw_stereo", 1);
+ }
+
+ void rawStereoCb(const stereo_msgs::RawStereoConstPtr& msg)
+ {
+ stereo_msgs::RawStereo next_raw_stereo = *msg;
+ next_raw_stereo.header.frame_id = msg->header.frame_id + frame_suffix_;
+ raw_stereo_pub_.publish(next_raw_stereo);
+
+ publishTransform(msg->header.stamp, next_raw_stereo.header.frame_id, msg->header.frame_id);
+ }
+
+private:
+ ros::NodeHandle nh_;
+ ros::Subscriber raw_stereo_sub_;
+ ros::Publisher raw_stereo_pub_;
+
+ std::string cam_name_;
+} ;
+
+}
+
+using namespace camera_offsetter;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "stereo_offsetter");
+
+ StereoOffsetter offsetter;
+
+ ros::spin();
+}
Copied: pkg/trunk/stacks/calibration/camera_offsetter/test/test_stereo_offsetter.launch (from rev 23150, pkg/trunk/calibration_experimental/sandbox/camera_offsetter/test/test_stereo_offsetter.launch)
===================================================================
--- pkg/trunk/stacks/calibration/camera_offsetter/test/test_stereo_offsetter.launch (rev 0)
+++ pkg/trunk/stacks/calibration/camera_offsetter/test/test_stereo_offsetter.launch 2009-08-27 17:21:19 UTC (rev 23152)
@@ -0,0 +1,11 @@
+<launch>
+
+ <!-- node type="spacenav_node" pkg="spacenav_node" name="spacenav_node" / -->
+ <node type="stereo_offsetter" pkg="camera_offsetter" name="narrow_stereo_offsetter" output="screen">
+ <remap from="~virtual_twist" to="keyboard_twist" />
+ <param name="cam_name" type="string" value="/narrow_stereo" />
+ <param name="position_scaling" type="double" value=".0001" />
+ <param name="angular_scaling" type="double" value="1.0" />
+ </node>
+
+</launch>
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
