Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[15041] pkg/trunk
|
From: <mee...@us...> - 2009-05-07 18:17:02
|
Revision: 15041
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15041&view=rev
Author: meeussen
Date: 2009-05-07 18:16:56 +0000 (Thu, 07 May 2009)
Log Message:
-----------
move door message from robot_msgs to new package door_msgs
Modified Paths:
--------------
pkg/trunk/highlevel/doors/door_functions/CMakeLists.txt
pkg/trunk/highlevel/doors/door_functions/include/door_functions/door_functions.h
pkg/trunk/highlevel/doors/door_functions/manifest.xml
pkg/trunk/highlevel/doors/door_functions/src/door_functions.cpp
pkg/trunk/highlevel/doors/door_functions/test/test_door_functions.cpp
pkg/trunk/highlevel/doors/doors_core/CMakeLists.txt
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_push_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_release_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_touch_door.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_unlatch_handle.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h
pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_grasp_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_open_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_push_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_release_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_touch_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_runner_unlatch_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp
pkg/trunk/highlevel/doors/doors_core/src/action_unlatch_handle.cpp
pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/doors/doors_core/test/test_executive_functions.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_detector.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_doors_opener.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_handle_release.cpp
pkg/trunk/highlevel/doors/doors_core/test/trigger_push_door.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapter_utilities.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/topological_map.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapter_utilities.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/door_domain_constraints.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/topological_map.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/topological_map_tests.cc
pkg/trunk/highlevel/pr2_robot_actions/manifest.xml
pkg/trunk/highlevel/pr2_robot_actions/msg/DoorActionState.msg
pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/doors_detector.h
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/handle_detector.h
pkg/trunk/mapping/door_handle_detector/manifest.xml
pkg/trunk/mapping/door_handle_detector/scripts/find_door.py
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_handle_detector/srv/DoorsDetector.srv
pkg/trunk/mapping/door_handle_detector/srv/DoorsDetectorCloud.srv
pkg/trunk/mapping/door_handle_detector/test/door_checkerboard_detector.cpp
pkg/trunk/mapping/door_handle_detector/test/test_door_detection.cpp
pkg/trunk/mapping/door_handle_detector/test/test_door_detection_joystick.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/mapping/door_tracker/manifest.xml
pkg/trunk/mapping/door_tracker/msg/DoorDatabaseObject.msg
pkg/trunk/mapping/door_tracker/src/door_database.cpp
pkg/trunk/mapping/door_tracker/src/door_tracker.cpp
pkg/trunk/motion_planning/mpbench/manifest.xml
pkg/trunk/motion_planning/mpbench/src/setup.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/sbpl_environment.h
pkg/trunk/motion_planning/mpglue/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/include/sbpl_door_planner/environment_navxythetadoor.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/manifest.xml
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/include/sbpl_door_planner_action/sbpl_door_planner_action.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner_action/src/action_runner_sbpl_door_planner.cpp
pkg/trunk/vision/recognition_lambertian/manifest.xml
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/recognition_lambertian/srv/DoorsDetector.srv
pkg/trunk/vision/recognition_lambertian/srv/DoorsDetectorCloud.srv
pkg/trunk/world_models/topological_map/include/topological_map/door_info.h
pkg/trunk/world_models/topological_map/include/topological_map/topological_map.h
pkg/trunk/world_models/topological_map/include/topological_map/topological_map_impl.h
pkg/trunk/world_models/topological_map/manifest.xml
pkg/trunk/world_models/topological_map/src/generate_from_ground_truth.cpp
pkg/trunk/world_models/topological_map/src/tm_driver.cpp
pkg/trunk/world_models/topological_map/src/visualization.cpp
Added Paths:
-----------
pkg/trunk/highlevel/doors/door_msgs/
pkg/trunk/highlevel/doors/door_msgs/CMakeLists.txt
pkg/trunk/highlevel/doors/door_msgs/Makefile
pkg/trunk/highlevel/doors/door_msgs/manifest.xml
pkg/trunk/highlevel/doors/door_msgs/msg/
pkg/trunk/highlevel/doors/door_msgs/msg/Door.msg
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/Door.msg
Deleted: pkg/trunk/common/robot_msgs/msg/Door.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/Door.msg 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/common/robot_msgs/msg/Door.msg 2009-05-07 18:16:56 UTC (rev 15041)
@@ -1,27 +0,0 @@
-Header header
-robot_msgs/Point32 frame_p1 ## position of the door frame
-robot_msgs/Point32 frame_p2 ## position of the door frame
-robot_msgs/Point32 door_p1 ## Ground plane projection of a point on the plane of the door
-robot_msgs/Point32 door_p2 ## Ground plane projection of a point on the plane of the door
-robot_msgs/Point32 handle ## Position of the door handle
-float32 height ## Height of the door
-
-int32 UNKNOWN=0
-
-int32 HINGE_P1=1
-int32 HINGE_P2=2
-int32 hinge
-
-int32 ROT_DIR_CLOCKWISE=1
-int32 ROT_DIR_COUNTERCLOCKWISE=2
-int32 rot_dir
-
-int32 LOCKED=1
-int32 LATCHED=2
-int32 UNLATCHED=3
-int32 latch_state
-
-Vector3 travel_dir ## vector pointing in the direction the robot is going to travel through the door
-float32 weight ## @Sachin: what do we use this for?
-
-
Modified: pkg/trunk/highlevel/doors/door_functions/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/doors/door_functions/CMakeLists.txt 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/door_functions/CMakeLists.txt 2009-05-07 18:16:56 UTC (rev 15041)
@@ -11,28 +11,3 @@
rospack_add_executable(test_door_functions test/test_door_functions.cpp src/door_functions.cpp)
rospack_add_library(door_functions src/door_functions.cpp)
-# check for OpenMP
-include(CheckIncludeFile)
-include(CheckCXXCompilerFlag)
-include(CheckLibraryExists)
-
-if( WIN32 )
- CHECK_INCLUDE_FILE(omp.h HAVE_OMP_H)
- if( HAVE_OMP_H )
- message(STATUS "Using OpenMP")
- check_cxx_compiler_flag(/openmp HAVE_OPENMP)
-
- if( HAVE_OPENMP )
- add_definitions("/openmp")
- endif( HAVE_OPENMP )
- endif( HAVE_OMP_H )
-elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
- # check if compilers supports -fopenmp
- check_cxx_compiler_flag(-fopenmp HAVE_OPENMP)
- check_library_exists(gomp omp_get_num_threads "" HAS_GOMP_LIB)
-
- if( HAVE_OPENMP AND HAS_GOMP_LIB )
- add_definitions("-fopenmp")
- set(OPENMP_LFLAGS "-lgomp")
- endif( HAVE_OPENMP AND HAS_GOMP_LIB )
-endif( WIN32 )
Modified: pkg/trunk/highlevel/doors/door_functions/include/door_functions/door_functions.h
===================================================================
--- pkg/trunk/highlevel/doors/door_functions/include/door_functions/door_functions.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/door_functions/include/door_functions/door_functions.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -35,34 +35,33 @@
#ifndef DOOR_FUNCTIONS_H
#define DOOR_FUNCTIONS_H
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <tf/tf.h>
#include <string.h>
-#include <robot_msgs/Door.h>
#include <kdl/frames.hpp>
namespace door_functions{
/// get robot and gripper pose
- tf::Stamped<tf::Pose> getRobotPose(const robot_msgs::Door& door, double dist);
- tf::Stamped<tf::Pose> getGripperPose(const robot_msgs::Door& door, double angle, double dist);
+ tf::Stamped<tf::Pose> getRobotPose(const door_msgs::Door& door, double dist);
+ tf::Stamped<tf::Pose> getGripperPose(const door_msgs::Door& door, double angle, double dist);
/// get the door angle
- double getDoorAngle(const robot_msgs::Door& door);
+ double getDoorAngle(const door_msgs::Door& door);
double getVectorAngle(const KDL::Vector& v1, const KDL::Vector& v2);
- KDL::Vector getDoorNormal(const robot_msgs::Door& door);
- KDL::Vector getFrameNormal(const robot_msgs::Door& door);
+ KDL::Vector getDoorNormal(const door_msgs::Door& door);
+ KDL::Vector getFrameNormal(const door_msgs::Door& door);
/// convert door message from its original frame/time, to the goal frame at time::now.
- bool transformTo(const tf::Transformer& tf, const std::string& goal_frame, const robot_msgs::Door& door_in,
- robot_msgs::Door& door_out, const std::string& fixed_frame="odom_combined");
+ bool transformTo(const tf::Transformer& tf, const std::string& goal_frame, const door_msgs::Door& door_in,
+ door_msgs::Door& door_out, const std::string& fixed_frame="odom_combined");
bool transformPointTo(const tf::Transformer& tf, const std::string& source_frame, const std::string& goal_frame, const ros::Time& time_source,
const robot_msgs::Point32& point_in, robot_msgs::Point32& point_out, const std::string& fixed_frame, const ros::Time& time_goal);
bool transformVectorTo(const tf::Transformer& tf, const std::string& source_frame, const std::string& goal_frame, const ros::Time& time_source,
const robot_msgs::Vector3& point_in, robot_msgs::Vector3& point_out, const std::string& fixed_frame, const ros::Time& time_goal);
- std::ostream& operator<< (std::ostream& os, const robot_msgs::Door& d);
- std::vector<robot_msgs::Point> getPolygon(const robot_msgs::Door& door, const double &door_thickness);
+ std::ostream& operator<< (std::ostream& os, const door_msgs::Door& d);
+ std::vector<robot_msgs::Point> getPolygon(const door_msgs::Door& door, const double &door_thickness);
}
#endif
Modified: pkg/trunk/highlevel/doors/door_functions/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/door_functions/manifest.xml 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/door_functions/manifest.xml 2009-05-07 18:16:56 UTC (rev 15041)
@@ -10,7 +10,7 @@
<depend package="roscpp" />
<depend package="std_msgs" />
- <depend package="robot_msgs" />
+ <depend package="door_msgs" />
<depend package="kdl"/>
<depend package="tf"/>
<depend package="gtest"/>
Modified: pkg/trunk/highlevel/doors/door_functions/src/door_functions.cpp
===================================================================
--- pkg/trunk/highlevel/doors/door_functions/src/door_functions.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/door_functions/src/door_functions.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -45,7 +45,7 @@
static const double eps_angle = 5.0*M_PI/180.0;
static const double gripper_height = 0.8;
- tf::Stamped<tf::Pose> getRobotPose(const robot_msgs::Door& door, double dist)
+ tf::Stamped<tf::Pose> getRobotPose(const door_msgs::Door& door, double dist)
{
Vector x_axis(1,0,0);
@@ -67,17 +67,17 @@
}
- tf::Stamped<tf::Pose> getGripperPose(const robot_msgs::Door& door, double angle, double dist)
+ tf::Stamped<tf::Pose> getGripperPose(const door_msgs::Door& door, double angle, double dist)
{
Vector x_axis(1,0,0);
// get hinge point
Vector hinge, frame_vec;
- if (door.hinge == robot_msgs::Door::HINGE_P1){
+ if (door.hinge == door_msgs::Door::HINGE_P1){
hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z);
frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z);
}
- else if (door.hinge == robot_msgs::Door::HINGE_P2){
+ else if (door.hinge == door_msgs::Door::HINGE_P2){
hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z);
frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z);
}
@@ -98,17 +98,17 @@
return gripper_pose;
}
- double getDoorAngle(const robot_msgs::Door& door)
+ double getDoorAngle(const door_msgs::Door& door)
{
Vector frame_vec(door.frame_p1.x-door.frame_p2.x, door.frame_p1.y-door.frame_p2.y, door.frame_p1.z-door.frame_p2.z);
Vector door_vec(door.door_p1.x-door.door_p2.x, door.door_p1.y-door.door_p2.y, door.door_p1.z-door.door_p2.z);
double angle = getVectorAngle(frame_vec, door_vec);
// validity check
- if (door.rot_dir == robot_msgs::Door::ROT_DIR_CLOCKWISE && angle > eps_angle)
+ if (door.rot_dir == door_msgs::Door::ROT_DIR_CLOCKWISE && angle > eps_angle)
ROS_ERROR("Door angle is positive, but door message specifies it turns clockwise");
- if (door.rot_dir == robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE && angle < -eps_angle)
+ if (door.rot_dir == door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE && angle < -eps_angle)
ROS_ERROR("Door angle is negative, but door message specifies it turns counter-clockwise");
return angle;
@@ -125,7 +125,7 @@
}
- Vector getDoorNormal(const robot_msgs::Door& door)
+ Vector getDoorNormal(const door_msgs::Door& door)
{
Vector frame_normal = getFrameNormal(door);
Rotation rot_frame_door = Rotation::RotZ(getDoorAngle(door));
@@ -133,7 +133,7 @@
}
- Vector getFrameNormal(const robot_msgs::Door& door)
+ Vector getFrameNormal(const door_msgs::Door& door)
{
// normal on frame
Vector p12(door.frame_p1.x-door.frame_p2.x, door.frame_p1.y-door.frame_p2.y, door.frame_p1.z-door.frame_p2.z);
@@ -150,7 +150,7 @@
}
- bool transformTo(const tf::Transformer& tf, const string& goal_frame, const robot_msgs::Door& door_in, robot_msgs::Door& door_out, const std::string& fixed_frame)
+ bool transformTo(const tf::Transformer& tf, const string& goal_frame, const door_msgs::Door& door_in, door_msgs::Door& door_out, const std::string& fixed_frame)
{
door_out = door_in;
ros::Time time_now = ros::Time::now();
@@ -193,7 +193,7 @@
return true;
}
- std::vector<robot_msgs::Point> getPolygon(const robot_msgs::Door& door, const double &door_thickness)
+ std::vector<robot_msgs::Point> getPolygon(const door_msgs::Door& door, const double &door_thickness)
{
std::vector<robot_msgs::Point> door_polygon;
robot_msgs::Point tmp;
@@ -216,7 +216,7 @@
return door_polygon;
}
- std::ostream& operator<< (std::ostream& os, const robot_msgs::Door& d)
+ std::ostream& operator<< (std::ostream& os, const door_msgs::Door& d)
{
os << "Door message in " << d.header.frame_id << " at time " << d.header.stamp.toSec() << endl;
os << " - frame ("
Modified: pkg/trunk/highlevel/doors/door_functions/test/test_door_functions.cpp
===================================================================
--- pkg/trunk/highlevel/doors/door_functions/test/test_door_functions.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/door_functions/test/test_door_functions.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -37,7 +37,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <ros/node.h>
#include <gtest/gtest.h>
#include <kdl/frames.hpp>
@@ -58,7 +58,7 @@
class TestEKF : public testing::Test
{
public:
- robot_msgs::Door my_door_1, my_door_2, my_door_3, my_door_4, my_door_5, my_door_6;
+ door_msgs::Door my_door_1, my_door_2, my_door_3, my_door_4, my_door_5, my_door_6;
protected:
/// constructor
@@ -75,8 +75,8 @@
my_door_1.travel_dir.x = 1.0;
my_door_1.travel_dir.y = 0.0;
my_door_1.travel_dir.z = 0.0;
- my_door_1.rot_dir = robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
- my_door_1.hinge = robot_msgs::Door::HINGE_P2;
+ my_door_1.rot_dir = door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
+ my_door_1.hinge = door_msgs::Door::HINGE_P2;
my_door_1.header.frame_id = "base_footprint";
my_door_2.frame_p2.x = 1.0;
@@ -90,8 +90,8 @@
my_door_2.travel_dir.x = 1.0;
my_door_2.travel_dir.y = 0.0;
my_door_2.travel_dir.z = 0.0;
- my_door_2.rot_dir = robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
- my_door_2.hinge = robot_msgs::Door::HINGE_P1;
+ my_door_2.rot_dir = door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
+ my_door_2.hinge = door_msgs::Door::HINGE_P1;
my_door_2.header.frame_id = "base_footprint";
my_door_3.frame_p1.x = 1.0;
@@ -105,8 +105,8 @@
my_door_3.travel_dir.x = 1.0;
my_door_3.travel_dir.y = 0.0;
my_door_3.travel_dir.z = 0.0;
- my_door_3.rot_dir = robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
- my_door_3.hinge = robot_msgs::Door::HINGE_P2;
+ my_door_3.rot_dir = door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
+ my_door_3.hinge = door_msgs::Door::HINGE_P2;
my_door_3.header.frame_id = "base_footprint";
my_door_4.frame_p2.x = 1.0;
@@ -120,8 +120,8 @@
my_door_4.travel_dir.x = 1.0;
my_door_4.travel_dir.y = 0.0;
my_door_4.travel_dir.z = 0.0;
- my_door_4.rot_dir = robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
- my_door_4.hinge = robot_msgs::Door::HINGE_P2;
+ my_door_4.rot_dir = door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE;
+ my_door_4.hinge = door_msgs::Door::HINGE_P2;
my_door_4.header.frame_id = "base_footprint";
my_door_5.frame_p1.x = 1.0;
@@ -135,8 +135,8 @@
my_door_5.travel_dir.x = 1.0;
my_door_5.travel_dir.y = 0.0;
my_door_5.travel_dir.z = 0.0;
- my_door_5.rot_dir = robot_msgs::Door::ROT_DIR_CLOCKWISE;
- my_door_5.hinge = robot_msgs::Door::HINGE_P2;
+ my_door_5.rot_dir = door_msgs::Door::ROT_DIR_CLOCKWISE;
+ my_door_5.hinge = door_msgs::Door::HINGE_P2;
my_door_5.header.frame_id = "base_footprint";
my_door_6.frame_p2.x = 1.0;
@@ -150,8 +150,8 @@
my_door_6.travel_dir.x = 1.0;
my_door_6.travel_dir.y = 0.0;
my_door_6.travel_dir.z = 0.0;
- my_door_6.rot_dir = robot_msgs::Door::ROT_DIR_CLOCKWISE;
- my_door_6.hinge = robot_msgs::Door::HINGE_P2;
+ my_door_6.rot_dir = door_msgs::Door::ROT_DIR_CLOCKWISE;
+ my_door_6.hinge = door_msgs::Door::HINGE_P2;
my_door_6.header.frame_id = "base_footprint";
}
Copied: pkg/trunk/highlevel/doors/door_msgs/CMakeLists.txt (from rev 15034, pkg/trunk/common/robot_msgs/CMakeLists.txt)
===================================================================
--- pkg/trunk/highlevel/doors/door_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/highlevel/doors/door_msgs/CMakeLists.txt 2009-05-07 18:16:56 UTC (rev 15041)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(door_msgs)
+genmsg()
+
+
Copied: pkg/trunk/highlevel/doors/door_msgs/Makefile (from rev 15034, pkg/trunk/common/robot_msgs/Makefile)
===================================================================
--- pkg/trunk/highlevel/doors/door_msgs/Makefile (rev 0)
+++ pkg/trunk/highlevel/doors/door_msgs/Makefile 2009-05-07 18:16:56 UTC (rev 15041)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/highlevel/doors/door_msgs/manifest.xml (from rev 15034, pkg/trunk/common/robot_msgs/manifest.xml)
===================================================================
--- pkg/trunk/highlevel/doors/door_msgs/manifest.xml (rev 0)
+++ pkg/trunk/highlevel/doors/door_msgs/manifest.xml 2009-05-07 18:16:56 UTC (rev 15041)
@@ -0,0 +1,14 @@
+<package>
+ <description brief="door_msgs">
+ Messages for door opening
+ </description>
+ <author>Wim Meeussen</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki</url>
+ <depend package="robot_msgs"/>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+</package>
+
Copied: pkg/trunk/highlevel/doors/door_msgs/msg/Door.msg (from rev 15035, pkg/trunk/common/robot_msgs/msg/Door.msg)
===================================================================
--- pkg/trunk/highlevel/doors/door_msgs/msg/Door.msg (rev 0)
+++ pkg/trunk/highlevel/doors/door_msgs/msg/Door.msg 2009-05-07 18:16:56 UTC (rev 15041)
@@ -0,0 +1,27 @@
+Header header
+robot_msgs/Point32 frame_p1 ## position of the door frame
+robot_msgs/Point32 frame_p2 ## position of the door frame
+robot_msgs/Point32 door_p1 ## Ground plane projection of a point on the plane of the door
+robot_msgs/Point32 door_p2 ## Ground plane projection of a point on the plane of the door
+robot_msgs/Point32 handle ## Position of the door handle
+float32 height ## Height of the door
+
+int32 UNKNOWN=0
+
+int32 HINGE_P1=1
+int32 HINGE_P2=2
+int32 hinge
+
+int32 ROT_DIR_CLOCKWISE=1
+int32 ROT_DIR_COUNTERCLOCKWISE=2
+int32 rot_dir
+
+int32 LOCKED=1
+int32 LATCHED=2
+int32 UNLATCHED=3
+int32 latch_state
+
+robot_msgs/Vector3 travel_dir ## vector pointing in the direction the robot is going to travel through the door
+float32 weight ## @Sachin: what do we use this for?
+
+
Modified: pkg/trunk/highlevel/doors/doors_core/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/CMakeLists.txt 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/CMakeLists.txt 2009-05-07 18:16:56 UTC (rev 15041)
@@ -90,30 +90,3 @@
src/move_base_door_action.cpp
src/door_reactive_planner.cpp
)
-
-
-# check for OpenMP
-include(CheckIncludeFile)
-include(CheckCXXCompilerFlag)
-include(CheckLibraryExists)
-
-if( WIN32 )
- CHECK_INCLUDE_FILE(omp.h HAVE_OMP_H)
- if( HAVE_OMP_H )
- message(STATUS "Using OpenMP")
- check_cxx_compiler_flag(/openmp HAVE_OPENMP)
-
- if( HAVE_OPENMP )
- add_definitions("/openmp")
- endif( HAVE_OPENMP )
- endif( HAVE_OMP_H )
-elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
- # check if compilers supports -fopenmp
- check_cxx_compiler_flag(-fopenmp HAVE_OPENMP)
- check_library_exists(gomp omp_get_num_threads "" HAS_GOMP_LIB)
-
- if( HAVE_OPENMP AND HAS_GOMP_LIB )
- add_definitions("-fopenmp")
- set(OPENMP_LFLAGS "-lgomp")
- endif( HAVE_OPENMP AND HAS_GOMP_LIB )
-endif( WIN32 )
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_door.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -39,26 +39,26 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <robot_actions/action.h>
#include <tf/transform_listener.h>
namespace door_handle_detector{
-class DetectDoorAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class DetectDoorAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
DetectDoorAction(ros::Node& node);
~DetectDoorAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
- robot_msgs::Door tmp_result_;
+ door_msgs::Door tmp_result_;
private:
- bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
+ bool laserDetection(const door_msgs::Door& door_in, door_msgs::Door& door_out);
tf::TransformListener tf_;
};
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_detect_handle.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -39,25 +39,25 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <robot_actions/action.h>
#include <tf/transform_listener.h>
namespace door_handle_detector{
-class DetectHandleAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class DetectHandleAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
DetectHandleAction(ros::Node& node);
~DetectHandleAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
- bool laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out);
- bool cameraDetection(const robot_msgs::Door& door, robot_msgs::Door& door_out);
+ bool laserDetection(const door_msgs::Door& door_in, door_msgs::Door& door_out);
+ bool cameraDetection(const door_msgs::Door& door, door_msgs::Door& door_out);
ros::Node& node_;
tf::TransformListener tf_;
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_handle.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_grasp_handle.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -40,7 +40,7 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
@@ -52,13 +52,13 @@
namespace door_handle_detector{
-class GraspHandleAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class GraspHandleAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
GraspHandleAction(ros::Node& node);
~GraspHandleAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_open_door.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -38,7 +38,7 @@
#define ACTION_OPEN_DOOR_H
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <robot_msgs/TaskFrameFormalism.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
@@ -51,13 +51,13 @@
namespace door_handle_detector{
-class OpenDoorAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class OpenDoorAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
OpenDoorAction(ros::Node& node);
~OpenDoorAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
ros::Node& node_;
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_push_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_push_door.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_push_door.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -40,7 +40,7 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
@@ -52,13 +52,13 @@
namespace door_handle_detector{
-class PushDoorAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class PushDoorAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
PushDoorAction(ros::Node& node);
~PushDoorAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_release_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_release_handle.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_release_handle.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -40,7 +40,7 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
@@ -54,13 +54,13 @@
namespace door_handle_detector{
-class ReleaseHandleAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class ReleaseHandleAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
ReleaseHandleAction(ros::Node& node);
~ReleaseHandleAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
void poseCallback();
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_touch_door.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_touch_door.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_touch_door.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -40,7 +40,7 @@
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
@@ -52,13 +52,13 @@
namespace door_handle_detector{
-class TouchDoorAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class TouchDoorAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
TouchDoorAction(ros::Node& node);
~TouchDoorAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_unlatch_handle.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_unlatch_handle.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/action_unlatch_handle.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -38,7 +38,7 @@
#define ACTION_UNLATCH_HANDLE_H
#include <ros/node.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <robot_msgs/TaskFrameFormalism.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
@@ -51,13 +51,13 @@
namespace door_handle_detector{
-class UnlatchHandleAction: public robot_actions::Action<robot_msgs::Door, robot_msgs::Door>
+class UnlatchHandleAction: public robot_actions::Action<door_msgs::Door, door_msgs::Door>
{
public:
UnlatchHandleAction(ros::Node& node);
~UnlatchHandleAction();
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
void tffCallback();
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/door_reactive_planner.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -41,7 +41,7 @@
#include <robot_msgs/Vector3.h>
#include <robot_msgs/Point.h>
#include <robot_msgs/DiagnosticStatus.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
// For transform support
#include <tf/transform_listener.h>
@@ -87,7 +87,7 @@
* @brief Set door information for the planner
* @param door_msg_in The door message containing information about the door
*/
- void setDoor(robot_msgs::Door door_msg_in);
+ void setDoor(door_msgs::Door door_msg_in);
/**
* @brief compute the oriented footprint for a particular position of the robot
Modified: pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/include/doors_core/move_base_door_action.h 2009-05-07 18:16:56 UTC (rev 15041)
@@ -49,7 +49,7 @@
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <robot_msgs/DiagnosticMessage.h>
#include <angles/angles.h>
@@ -63,7 +63,7 @@
* @class MoveBaseDoorAction
* @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
*/
- class MoveBaseDoorAction : public robot_actions::Action<robot_msgs::Door, robot_msgs::Door> {
+ class MoveBaseDoorAction : public robot_actions::Action<door_msgs::Door, door_msgs::Door> {
public:
/**
* @brief Constructor for the actions
@@ -84,7 +84,7 @@
* @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
* @return The result of the execution, ie: Success, Preempted, Aborted, etc.
*/
- virtual robot_actions::ResultStatus execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback);
+ virtual robot_actions::ResultStatus execute(const door_msgs::Door& goal, door_msgs::Door& feedback);
private:
/**
@@ -171,7 +171,7 @@
std::string global_frame_, control_frame_, robot_base_frame_;
bool valid_plan_;
boost::recursive_mutex lock_;
- robot_msgs::Door door_;
+ door_msgs::Door door_;
pr2_robot_actions::Pose2D goal_;
tf::Stamped<tf::Pose> global_pose_;
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-05-07 18:16:56 UTC (rev 15041)
@@ -11,6 +11,7 @@
<depend package="roscpp" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
+ <depend package="door_msgs" />
<depend package="robot_srvs" />
<depend package="nav_srvs" />
<depend package="robot_actions" />
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -51,7 +51,7 @@
DetectDoorAction::DetectDoorAction(Node& node):
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("detect_door"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("detect_door"),
tf_(node)
{};
@@ -59,19 +59,19 @@
DetectDoorAction::~DetectDoorAction(){};
-robot_actions::ResultStatus DetectDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus DetectDoorAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("DetectDoorAction: execute");
// transform door message to time fixed frame
- robot_msgs::Door goal_tr;
+ door_msgs::Door goal_tr;
if (!transformTo(tf_, fixed_frame, goal, goal_tr, fixed_frame)){
ROS_ERROR("DetectDoorAction: Could not tranform door message from '%s' to '%s' at time %f",
goal.header.frame_id.c_str(), fixed_frame.c_str(), goal.header.stamp.toSec());
return robot_actions::ABORTED;
}
ROS_INFO("DetectDoorAction: goal message transformed to frame %s", fixed_frame.c_str());
- robot_msgs::Door result_laser;
+ door_msgs::Door result_laser;
if (!laserDetection(goal_tr, result_laser)){
if (isPreemptRequested()){
ROS_ERROR("DetectDoorAction: Preempted");
@@ -88,7 +88,7 @@
return robot_actions::SUCCESS;
}
-bool DetectDoorAction::laserDetection(const robot_msgs::Door& door_in, robot_msgs::Door& door_out)
+bool DetectDoorAction::laserDetection(const door_msgs::Door& door_in, door_msgs::Door& door_out)
{
// check where robot is relative to door
if (isPreemptRequested()) return false;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_detect_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -37,7 +37,7 @@
#include <door_handle_detector/DoorsDetectorCloud.h>
#include <door_handle_detector/DoorsDetector.h>
#include <door_functions/door_functions.h>
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include <point_cloud_assembler/BuildCloudAngle.h>
#include "doors_core/action_detect_handle.h"
@@ -53,7 +53,7 @@
static const unsigned int max_retries = 7;
DetectHandleAction::DetectHandleAction(Node& node):
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("detect_handle"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("detect_handle"),
node_(node),
tf_(node)
{
@@ -68,7 +68,7 @@
-robot_actions::ResultStatus DetectHandleAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus DetectHandleAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("DetectHandleAction: execute");
@@ -76,7 +76,7 @@
feedback = goal;
// transform door message to time fixed frame
- robot_msgs::Door goal_tr;
+ door_msgs::Door goal_tr;
transformTo(tf_, fixed_frame, goal, goal_tr, fixed_frame);
// try to detect handle
@@ -89,7 +89,7 @@
}
// laser detection
- robot_msgs::Door result_laser, result_camera;
+ door_msgs::Door result_laser, result_camera;
if (!laserDetection(goal_tr, result_laser)){
ROS_ERROR("DetectHandleAction: Laser detection failed on try %i", nr_tries+1);
continue;
@@ -150,12 +150,12 @@
// store hinge side
/*
- if (feedback.hinge == robot_msgs::Door::UNKNOWN){
+ if (feedback.hinge == door_msgs::Door::UNKNOWN){
if (pow(feedback.handle.x-result_laser.door_p1.x,2)+pow(feedback.handle.y-result_laser.door_p1.y,2) <
pow(feedback.handle.x-result_laser.door_p2.x,2)+pow(feedback.handle.y-result_laser.door_p2.y,2))
- feedback.hinge = robot_msgs::Door::HINGE_P2;
+ feedback.hinge = door_msgs::Door::HINGE_P2;
else
- feedback.hinge = robot_msgs::Door::HINGE_P1;
+ feedback.hinge = door_msgs::Door::HINGE_P1;
}
*/
ROS_INFO("DetectHandleAction: Found handle in %i tries", nr_tries+1);
@@ -169,8 +169,8 @@
-bool DetectHandleAction::laserDetection(const robot_msgs::Door& door_in,
- robot_msgs::Door& door_out)
+bool DetectHandleAction::laserDetection(const door_msgs::Door& door_in,
+ door_msgs::Door& door_out)
{
// check where robot is relative to the door
if (isPreemptRequested()) return false;
@@ -225,8 +225,8 @@
return true;
}
-bool DetectHandleAction::cameraDetection(const robot_msgs::Door& door_in,
- robot_msgs::Door& door_out)
+bool DetectHandleAction::cameraDetection(const door_msgs::Door& door_in,
+ door_msgs::Door& door_out)
{
// make the head point towards the door
if (isPreemptRequested()) return false;
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_grasp_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -50,7 +50,7 @@
GraspHandleAction::GraspHandleAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("grasp_handle"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("grasp_handle"),
node_(node),
tf_(node)
{
@@ -63,7 +63,7 @@
-robot_actions::ResultStatus GraspHandleAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus GraspHandleAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("GraspHandleAction: execute");
@@ -71,7 +71,7 @@
feedback = goal;
// transform door message to time fixed frame
- robot_msgs::Door goal_tr;
+ door_msgs::Door goal_tr;
if (!transformTo(tf_, fixed_frame, goal, goal_tr, fixed_frame)){
ROS_ERROR("GraspHandleAction: Could not tranform door message from '%s' to '%s' at time %f",
goal.header.frame_id.c_str(), fixed_frame.c_str(), goal.header.stamp.toSec());
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_open_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -47,7 +47,7 @@
OpenDoorAction::OpenDoorAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("open_door"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("open_door"),
node_(node)
{
node_.advertise<robot_msgs::TaskFrameFormalism>("r_arm_cartesian_tff_controller/command", 10);
@@ -62,7 +62,7 @@
-robot_actions::ResultStatus OpenDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus OpenDoorAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("OpenDoorAction: execute");
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_push_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -52,7 +52,7 @@
PushDoorAction::PushDoorAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("push_door"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("push_door"),
node_(node),
tf_(node)
{
@@ -69,7 +69,7 @@
-robot_actions::ResultStatus PushDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus PushDoorAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("PushDoorAction: execute");
@@ -77,7 +77,7 @@
feedback = goal;
// transform door message to time fixed frame
- robot_msgs::Door goal_tr;
+ door_msgs::Door goal_tr;
if (!transformTo(tf_, fixed_frame, goal, goal_tr, fixed_frame)){
ROS_ERROR("PushDoorAction: Could not tranform door message from '%s' to '%s' at time %f",
goal.header.frame_id.c_str(), fixed_frame.c_str(), goal.header.stamp.toSec());
@@ -92,9 +92,9 @@
// angle step
Duration sleep_time(0.01);
double angle_step, angle=0;
- if (goal_tr.rot_dir == robot_msgs::Door::ROT_DIR_CLOCKWISE)
+ if (goal_tr.rot_dir == door_msgs::Door::ROT_DIR_CLOCKWISE)
angle_step = -push_vel*sleep_time.toSec();
- else if (goal_tr.rot_dir == robot_msgs::Door::ROT_DIR_COUNTERCLOCKWISE)
+ else if (goal_tr.rot_dir == door_msgs::Door::ROT_DIR_COUNTERCLOCKWISE)
angle_step = push_vel*sleep_time.toSec();
else{
ROS_ERROR("PushDoorAction: door rotation direction not specified");
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_release_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -48,7 +48,7 @@
ReleaseHandleAction::ReleaseHandleAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("release_handle"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("release_handle"),
node_(node),
tf_(node)
{
@@ -61,7 +61,7 @@
-robot_actions::ResultStatus ReleaseHandleAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus ReleaseHandleAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("ReleaseHandleAction: execute");
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_detect_door.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
DetectDoorAction detect_door(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(detect_door);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(detect_door);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_detect_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_detect_handle.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
DetectHandleAction detect_handle(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(detect_handle);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(detect_handle);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_grasp_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_grasp_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_grasp_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_grasp_handle.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
GraspHandleAction grasp(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(grasp);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(grasp);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_open_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_open_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_open_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_open_door.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
OpenDoorAction open(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(open);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(open);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_push_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_push_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_push_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_push_door.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
PushDoorAction push(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(push);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(push);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_release_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_release_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_release_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_release_handle.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
ReleaseHandleAction release(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(release);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(release);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_touch_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_touch_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_touch_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_touch_door.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
TouchDoorAction touch(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(touch);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(touch);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_runner_unlatch_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_runner_unlatch_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_runner_unlatch_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -34,7 +34,7 @@
/* Author: Wim Meeussen */
-#include <robot_msgs/Door.h>
+#include <door_msgs/Door.h>
#include "doors_core/action_unlatch_handle.h"
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
@@ -57,7 +57,7 @@
UnlatchHandleAction unlatch(node);
robot_actions::ActionRunner runner(10.0);
- runner.connect<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door>(unlatch);
+ runner.connect<door_msgs::Door, pr2_robot_actions::DoorActionState, door_msgs::Door>(unlatch);
runner.run();
node.spin();
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_touch_door.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -50,7 +50,7 @@
TouchDoorAction::TouchDoorAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("touch_door"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("touch_door"),
node_(node),
tf_(node)
{
@@ -65,7 +65,7 @@
-robot_actions::ResultStatus TouchDoorAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus TouchDoorAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("TouchDoorAction: execute");
@@ -73,7 +73,7 @@
feedback = goal;
// transform door message to time fixed frame
- robot_msgs::Door goal_tr;
+ door_msgs::Door goal_tr;
if (!transformTo(tf_, fixed_frame, goal, goal_tr, fixed_frame)){
ROS_ERROR("TouchDoorAction: Could not tranform door message from '%s' to '%s' at time %f",
goal.header.frame_id.c_str(), fixed_frame.c_str(), goal.header.stamp.toSec());
Modified: pkg/trunk/highlevel/doors/doors_core/src/action_unlatch_handle.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/action_unlatch_handle.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/action_unlatch_handle.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -47,7 +47,7 @@
UnlatchHandleAction::UnlatchHandleAction(Node& node) :
- robot_actions::Action<robot_msgs::Door, robot_msgs::Door>("unlatch_handle"),
+ robot_actions::Action<door_msgs::Door, door_msgs::Door>("unlatch_handle"),
node_(node)
{
node_.advertise<robot_msgs::TaskFrameFormalism>("r_arm_cartesian_tff_controller/command", 10);
@@ -62,7 +62,7 @@
-robot_actions::ResultStatus UnlatchHandleAction::execute(const robot_msgs::Door& goal, robot_msgs::Door& feedback)
+robot_actions::ResultStatus UnlatchHandleAction::execute(const door_msgs::Door& goal, door_msgs::Door& feedback)
{
ROS_INFO("UnlatchHandleAction: execute");
@@ -147,7 +147,7 @@
node_.unsubscribe("r_arm_cartesian_tff_controller/state/position");
node_.publish("r_arm_cartesian_tff_controller/command", tff_stop_);
ROS_INFO("UnlatchHandleAction: Door is locked");
- feedback.latch_state = robot_msgs::Door::LOCKED;
+ feedback.latch_state = door_msgs::Door::LOCKED;
return robot_actions::SUCCESS;
}
@@ -161,7 +161,7 @@
// finished
node_.unsubscribe("r_arm_cartesian_tff_controller/state/position");
- feedback.latch_state = robot_msgs::Door::UNLATCHED;
+ feedback.latch_state = door_msgs::Door::UNLATCHED;
return robot_actions::SUCCESS;
}
Modified: pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/door_reactive_planner.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -96,10 +96,10 @@
footprint_.push_back(pt);
}
-void DoorReactivePlanner::setDoor(robot_msgs::Door door_msg_in)
+void DoorReactivePlanner::setDoor(door_msgs::Door door_msg_in)
{
//Assumption is that the normal points in the direction we want to travel through the door
- robot_msgs::Door door;
+ door_msgs::Door door;
if (!transformTo(tf_,costmap_frame_id_,door_msg_in,door)){
ROS_ERROR("DoorReactivePlanner: Could not transform door message from %s to %s", door_msg_in.header.frame_id.c_str(), costmap_frame_id_.c_str());
return;
Modified: pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-05-07 17:29:40 UTC (rev 15040)
+++ pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp 2009-05-07 18:16:56 UTC (rev 15041)
@@ -49,7 +49,7 @@
namespace nav
{
MoveBaseDoorAction::MoveBaseDoorAction(ros::Node& ros_node, tf::TransformListener& tf) :
- Action<robot_msgs::Door, robot_msgs::D...
[truncated message content] |
