Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2009-06-05 23:40:13
|
Revision: 16796
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16796&view=rev
Author: tfoote
Date: 2009-06-05 23:40:09 +0000 (Fri, 05 Jun 2009)
Log Message:
-----------
robot_msgs/Diagnostic* to diagnostic_msgs/Diagnostic* and robot_srvs/SelfTest into diagnostic_msgs too
Modified Paths:
--------------
pkg/trunk/calibration/joint_calibration_monitor/scripts/pr2_calibration_monitor_node.py
pkg/trunk/common/diagnostic_msgs/CMakeLists.txt
pkg/trunk/common/diagnostic_msgs/manifest.xml
pkg/trunk/common/robot_actions/include/robot_actions/action.h
pkg/trunk/common/robot_actions/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/stereodcam.cpp
pkg/trunk/drivers/cam/prosilica_cam/manifest.xml
pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_node.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/laser/hokuyo_node/src/node/hokuyo_node.cpp
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ek1122.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_device.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg014.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ek1122.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg014.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/manifest.xml
pkg/trunk/drivers/robot/pr2/ocean_battery_driver/scripts/monitor
pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_power_board/scripts/view_power
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/power_node/power_node_simulator.cpp
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/pr2_power_board/power_wires.py
pkg/trunk/drivers/robot/pr2/pr2_power_board/src/pr2_power_board/pr2_power_board_panel.py
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/DiagnosticStatusWrapper.h
pkg/trunk/hardware_test/diagnostic_updater/include/diagnostic_updater/diagnostic_updater.h
pkg/trunk/hardware_test/diagnostic_updater/manifest.xml
pkg/trunk/hardware_test/diagnostic_updater/test/diagnostic_updater_test.cpp
pkg/trunk/hardware_test/runtime_monitor/manifest.xml
pkg/trunk/hardware_test/runtime_monitor/scripts/log_runtime_messages
pkg/trunk/hardware_test/runtime_monitor/scripts/monitor
pkg/trunk/hardware_test/runtime_monitor/scripts/ntp_monitor.py
pkg/trunk/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/hardware_test/runtime_monitor/scripts/test_high_level
pkg/trunk/hardware_test/runtime_monitor/src/runtime_monitor/monitor_panel.py
pkg/trunk/hardware_test/runtime_monitor/testmonitor.py
pkg/trunk/hardware_test/self_test/CMakeLists.txt
pkg/trunk/hardware_test/self_test/include/self_test/self_test.h
pkg/trunk/hardware_test/self_test/manifest.xml
pkg/trunk/hardware_test/self_test/src/run_selftest.cpp
pkg/trunk/hardware_test/self_test/src/selftest_example.cpp
pkg/trunk/hardware_test/self_test/src/selftest_rostest.cpp
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/door_reactive_planner.cpp
pkg/trunk/highlevel/doors/doors_core/src/move_base_door_action.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/pr2/life_test/manifest.xml
pkg/trunk/pr2/life_test/simple_test/fake_test.py
pkg/trunk/pr2/life_test/src/life_test/life_test.py
pkg/trunk/pr2/pr2_etherCAT/manifest.xml
pkg/trunk/pr2/pr2_etherCAT/src/main.cpp
pkg/trunk/pr2/qualification/manifest.xml
pkg/trunk/pr2/qualification/scripts/robot_checkout.py
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/motor_monitor.py
pkg/trunk/vision/stereo_image_proc/manifest.xml
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
Added Paths:
-----------
pkg/trunk/common/diagnostic_msgs/srv/
pkg/trunk/common/diagnostic_msgs/srv/SelfTest.srv
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/DiagnosticMessage.msg
pkg/trunk/common/robot_msgs/msg/DiagnosticStatus.msg
pkg/trunk/common/robot_msgs/msg/DiagnosticString.msg
pkg/trunk/common/robot_msgs/msg/DiagnosticValue.msg
Modified: pkg/trunk/calibration/joint_calibration_monitor/scripts/pr2_calibration_monitor_node.py
===================================================================
--- pkg/trunk/calibration/joint_calibration_monitor/scripts/pr2_calibration_monitor_node.py 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/calibration/joint_calibration_monitor/scripts/pr2_calibration_monitor_node.py 2009-06-05 23:40:09 UTC (rev 16796)
@@ -41,7 +41,7 @@
import threading
import math
-from robot_msgs.msg import *
+from diagnostic_msgs.msg import DiagnosticMessage
from roslib import rostime
from joint_calibration_monitor.generic_joint_monitor import *
Modified: pkg/trunk/common/diagnostic_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/common/diagnostic_msgs/CMakeLists.txt 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/diagnostic_msgs/CMakeLists.txt 2009-06-05 23:40:09 UTC (rev 16796)
@@ -19,7 +19,7 @@
#uncomment if you have defined messages
genmsg()
#uncomment if you have defined services
-#gensrv()
+gensrv()
#common commands for building c++ executables and libraries
#rospack_add_library(${PROJECT_NAME} src/example.cpp)
Modified: pkg/trunk/common/diagnostic_msgs/manifest.xml
===================================================================
--- pkg/trunk/common/diagnostic_msgs/manifest.xml 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/diagnostic_msgs/manifest.xml 2009-06-05 23:40:09 UTC (rev 16796)
@@ -9,7 +9,7 @@
<review status="api cleared" notes=""/>
<url>http://pr.willowgarage.com/wiki/diagnostic_msgs</url>
<export>
- <cpp cflags="-I${prefix}/msg/cpp"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/srv/cpp"/>
</export>
</package>
Copied: pkg/trunk/common/diagnostic_msgs/srv/SelfTest.srv (from rev 14615, pkg/trunk/common/robot_srvs/srv/SelfTest.srv)
===================================================================
--- pkg/trunk/common/diagnostic_msgs/srv/SelfTest.srv (rev 0)
+++ pkg/trunk/common/diagnostic_msgs/srv/SelfTest.srv 2009-06-05 23:40:09 UTC (rev 16796)
@@ -0,0 +1,4 @@
+---
+string id
+byte passed
+DiagnosticStatus[] status
Modified: pkg/trunk/common/robot_actions/include/robot_actions/action.h
===================================================================
--- pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_actions/include/robot_actions/action.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -50,7 +50,7 @@
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <ros/node.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
namespace robot_actions {
@@ -185,7 +185,7 @@
Action(const std::string& name)
: _name(name), _preempt_request(false), _result_status(SUCCESS), _terminated(false), _action_thread(NULL), _callback(NULL),_diagnostics_statuses(1){
_status.value = ActionStatus::UNDEFINED;
- ros::Node::instance()->advertise<robot_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
+ ros::Node::instance()->advertise<diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
}
virtual ~Action(){
@@ -263,7 +263,7 @@
}
void publishDiagnostics(){
- robot_msgs::DiagnosticStatus diagnostics_status;
+ diagnostic_msgs::DiagnosticStatus diagnostics_status;
if (_status.value == ActionStatus::ACTIVE){
if (isPreemptRequested())
diagnostics_status.message = "Active and requested preempt";
@@ -300,8 +300,8 @@
bool _terminated;
boost::thread* _action_thread; /*!< Thread running the action */
boost::function< void(const ActionStatus&, const Goal&, const Feedback&) > _callback; /*!< Callback function for sending updates */
- robot_msgs::DiagnosticMessage _diagnostics_message;
- std::vector<robot_msgs::DiagnosticStatus> _diagnostics_statuses;
+ diagnostic_msgs::DiagnosticMessage _diagnostics_message;
+ std::vector<diagnostic_msgs::DiagnosticStatus> _diagnostics_statuses;
};
}
#endif
Modified: pkg/trunk/common/robot_actions/manifest.xml
===================================================================
--- pkg/trunk/common/robot_actions/manifest.xml 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_actions/manifest.xml 2009-06-05 23:40:09 UTC (rev 16796)
@@ -7,6 +7,7 @@
<depend package="roscpp"/>
<depend package="robot_msgs"/>
+ <depend package="diagnostic_msgs"/>
<depend package="std_msgs"/>
<depend package="rospy" />
<depend package="rostopic" />
Deleted: pkg/trunk/common/robot_msgs/msg/DiagnosticMessage.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/DiagnosticMessage.msg 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_msgs/msg/DiagnosticMessage.msg 2009-06-05 23:40:09 UTC (rev 16796)
@@ -1,2 +0,0 @@
-Header header #for timestamp
-DiagnosticStatus[] status # an array of components being reported on
\ No newline at end of file
Deleted: pkg/trunk/common/robot_msgs/msg/DiagnosticStatus.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/DiagnosticStatus.msg 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_msgs/msg/DiagnosticStatus.msg 2009-06-05 23:40:09 UTC (rev 16796)
@@ -1,5 +0,0 @@
-byte level #(OK=0, WARN=1, ERROR=2)
-string name # a description of the test/component reporting
-string message # a description of the status
-DiagnosticValue[] values # an array of values associated with the status
-DiagnosticString[] strings # an array of string associated with the status
Deleted: pkg/trunk/common/robot_msgs/msg/DiagnosticString.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/DiagnosticString.msg 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_msgs/msg/DiagnosticString.msg 2009-06-05 23:40:09 UTC (rev 16796)
@@ -1,2 +0,0 @@
-string value # a string data type
-string label # what to label this value when viewing
Deleted: pkg/trunk/common/robot_msgs/msg/DiagnosticValue.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/DiagnosticValue.msg 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/common/robot_msgs/msg/DiagnosticValue.msg 2009-06-05 23:40:09 UTC (rev 16796)
@@ -1,2 +0,0 @@
-float32 value # a value to track over time
-string label # what to label this value when viewing
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -58,7 +58,7 @@
#include <robot_msgs/JointTraj.h>
#include <robot_msgs/JointTrajPoint.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
@@ -240,7 +240,7 @@
void publishDiagnostics();
- realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ; //!< Publishes controller information
+ realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage>* diagnostics_publisher_ ; //!< Publishes controller information
/*!
* \brief mutex lock for setting and getting ros messages
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -43,7 +43,7 @@
// Services
#include <robot_msgs/JointTraj.h>
#include <robot_msgs/JointTrajPoint.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
@@ -182,7 +182,7 @@
bool watch_dog_active_;
- realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ; /**< Publishes controller information as a diagnostic message */
+ realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage>* diagnostics_publisher_ ; /**< Publishes controller information as a diagnostic message */
/*!
* \brief mutex lock for setting and getting ros messages
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -58,7 +58,7 @@
#include <robot_msgs/JointTraj.h>
#include <robot_msgs/JointTrajPoint.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
@@ -287,7 +287,7 @@
void publishDiagnostics();
- realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ; //!< Publishes controller information
+ realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage>* diagnostics_publisher_ ; //!< Publishes controller information
/*!
* \brief mutex lock for setting and getting ros messages
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -409,7 +409,7 @@
if (diagnostics_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete diagnostics_publisher_ ;
- diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
+ diagnostics_publisher_ = new realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
@@ -689,11 +689,11 @@
cmd.set_positions_size(1);
cmd.set_velocity_size(1);
- vector<robot_msgs::DiagnosticStatus> statuses;
- vector<robot_msgs::DiagnosticValue> values;
- vector<robot_msgs::DiagnosticString> strings;
- robot_msgs::DiagnosticStatus status;
- robot_msgs::DiagnosticValue v;
+ vector<diagnostic_msgs::DiagnosticStatus> statuses;
+ vector<diagnostic_msgs::DiagnosticValue> values;
+ vector<diagnostic_msgs::DiagnosticString> strings;
+ diagnostic_msgs::DiagnosticStatus status;
+ diagnostic_msgs::DiagnosticValue v;
status.name = service_prefix_;
status.level = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_trajectory_controller.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -35,8 +35,8 @@
* Author: Sachin Chitta
*********************************************************************/
#include <pr2_mechanism_controllers/base_trajectory_controller.h>
-#include <robot_msgs/DiagnosticMessage.h>
-#include <robot_msgs/DiagnosticStatus.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticStatus.h>
#include <ros/rate.h>
namespace pr2_mechanism_controllers
@@ -83,7 +83,7 @@
ros_node_.advertise<robot_msgs::PoseDot>(control_topic_name_, 1);
ros_node_.subscribe(path_input_topic_name_,path_msg_in_, &BaseTrajectoryController::pathCallback, this, 1);
- ros_node_.advertise<robot_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
+ ros_node_.advertise<diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 1) ;
last_diagnostics_publish_time_ = ros::Time::now();
current_time_ = ros::Time::now().toSec();
@@ -277,14 +277,14 @@
return;
}
- robot_msgs::DiagnosticMessage message;
- std::vector<robot_msgs::DiagnosticStatus> statuses;
- std::vector<robot_msgs::DiagnosticValue> values;
- std::vector<robot_msgs::DiagnosticString> strings;
+ diagnostic_msgs::DiagnosticMessage message;
+ std::vector<diagnostic_msgs::DiagnosticStatus> statuses;
+ std::vector<diagnostic_msgs::DiagnosticValue> values;
+ std::vector<diagnostic_msgs::DiagnosticString> strings;
- robot_msgs::DiagnosticStatus status;
- robot_msgs::DiagnosticValue v;
- robot_msgs::DiagnosticString s;
+ diagnostic_msgs::DiagnosticStatus status;
+ diagnostic_msgs::DiagnosticValue v;
+ diagnostic_msgs::DiagnosticString s;
status.name = ros_node_.getName();
status.message = control_state_;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -129,7 +129,7 @@
if (diagnostics_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete diagnostics_publisher_ ;
- diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
+ diagnostics_publisher_ = new realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
last_diagnostics_publish_time_ = robot_->hw_->current_time_;
node_->param<double>(prefix_+"diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
@@ -1018,12 +1018,12 @@
cmd.set_positions_size(1);
cmd.set_velocity_size(1);
- vector<robot_msgs::DiagnosticStatus> statuses;
- vector<robot_msgs::DiagnosticValue> values;
- vector<robot_msgs::DiagnosticString> strings;
- robot_msgs::DiagnosticStatus status;
- robot_msgs::DiagnosticValue v;
- robot_msgs::DiagnosticString s;
+ vector<diagnostic_msgs::DiagnosticStatus> statuses;
+ vector<diagnostic_msgs::DiagnosticValue> values;
+ vector<diagnostic_msgs::DiagnosticString> strings;
+ diagnostic_msgs::DiagnosticStatus status;
+ diagnostic_msgs::DiagnosticValue v;
+ diagnostic_msgs::DiagnosticString s;
status.name = "Whole Body Trajectory Controller";
status.level = 0;
if(watch_dog_active_)
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -639,7 +639,7 @@
if (diagnostics_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete diagnostics_publisher_ ;
- diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
+ diagnostics_publisher_ = new realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
@@ -914,11 +914,11 @@
cmd.set_positions_size(1);
cmd.set_velocity_size(1);
- vector<robot_msgs::DiagnosticStatus> statuses;
- vector<robot_msgs::DiagnosticValue> values;
- vector<robot_msgs::DiagnosticString> strings;
- robot_msgs::DiagnosticStatus status;
- robot_msgs::DiagnosticValue v;
+ vector<diagnostic_msgs::DiagnosticStatus> statuses;
+ vector<diagnostic_msgs::DiagnosticValue> values;
+ vector<diagnostic_msgs::DiagnosticString> strings;
+ diagnostic_msgs::DiagnosticStatus status;
+ diagnostic_msgs::DiagnosticValue v;
status.name = "Whole Body Trajectory Controller";
status.level = 0;
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/backlash_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -45,7 +45,7 @@
#include <ros/node.h>
#include <math.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
#include <mechanism_model/controller.h>
@@ -87,7 +87,7 @@
inline bool done() { return done_; }
- robot_msgs::DiagnosticMessage diagnostic_message_;
+ diagnostic_msgs::DiagnosticMessage diagnostic_message_;
joint_qualification_controllers::TestData::Request test_data_;
private:
@@ -127,7 +127,7 @@
double last_publish_time_;
realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> call_service_;
- realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> pub_diagnostics_;
+ realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticMessage> pub_diagnostics_;
};
}
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hold_set_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -48,7 +48,6 @@
#include <vector>
#include <ros/node.h>
#include <math.h>
-#include <robot_msgs/DiagnosticMessage.h>
#include <joint_qualification_controllers/HoldSetData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
Modified: pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/qualification_controllers/joint_qualification_controllers/include/joint_qualification_controllers/hysteresis_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -48,7 +48,7 @@
#include <ros/node.h>
#include <math.h>
#include <sstream>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <joint_qualification_controllers/TestData.h>
#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_srv_call.h>
@@ -90,7 +90,7 @@
bool done() { return state_ == DONE; }
- robot_msgs::DiagnosticMessage diagnostic_message_;
+ diagnostic_msgs::DiagnosticMessage diagnostic_message_;
joint_qualification_controllers::TestData::Request test_data_;
private:
@@ -143,7 +143,7 @@
double last_publish_time_;
realtime_tools::RealtimeSrvCall<joint_qualification_controllers::TestData::Request, joint_qualification_controllers::TestData::Response> call_service_;
- //realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> pub_diagnostics_;
+ //realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticMessage> pub_diagnostics_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_wrench_controller.h 2009-06-05 23:40:09 UTC (rev 16796)
@@ -43,7 +43,7 @@
#include <mechanism_model/controller.h>
#include <mechanism_model/chain.h>
#include <tf/transform_datatypes.h>
-#include <robot_msgs/DiagnosticMessage.h>
+#include <diagnostic_msgs/DiagnosticMessage.h>
#include <realtime_tools/realtime_publisher.h>
#include <boost/scoped_ptr.hpp>
@@ -79,8 +79,8 @@
KDL::JntArray jnt_pos_, jnt_eff_;
KDL::Jacobian jacobian_;
- robot_msgs::DiagnosticMessage diagnostics_;
- realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> diagnostics_publisher_;
+ diagnostic_msgs::DiagnosticMessage diagnostics_;
+ realtime_tools::RealtimePublisher <diagnostic_msgs::DiagnosticMessage> diagnostics_publisher_;
ros::Time diagnostics_time_;
ros::Duration diagnostics_interval_;
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -142,7 +142,7 @@
}
- void checkImage(robot_msgs::DiagnosticStatus& status)
+ void checkImage(diagnostic_msgs::DiagnosticStatus& status)
{
status.name = "Image Test";
uint8_t *jpeg;
@@ -178,7 +178,7 @@
}
}
- void checkMac(robot_msgs::DiagnosticStatus& status)
+ void checkMac(diagnostic_msgs::DiagnosticStatus& status)
{
status.name = "MAC test";
char cmd[100];
Modified: pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-06-05 22:36:30 UTC (rev 16795)
+++ pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp 2009-06-05 23:40:09 UTC (rev 16796)
@@ -311,7 +311,7 @@
}
- void freqStatus(robot_msgs::DiagnosticStatus& status)
+ void freqStatus(diagnostic_msgs::D...
[truncated message content] |
|
From: <is...@us...> - 2009-06-06 06:08:38
|
Revision: 16809
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16809&view=rev
Author: isucan
Date: 2009-06-06 06:08:33 +0000 (Sat, 06 Jun 2009)
Log Message:
-----------
new filter for removing seen parts of self
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
Added Paths:
-----------
pkg/trunk/util/new_robot_self_filter/
pkg/trunk/util/new_robot_self_filter/.build_version
pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
pkg/trunk/util/new_robot_self_filter/Makefile
pkg/trunk/util/new_robot_self_filter/mainpage.dox
pkg/trunk/util/new_robot_self_filter/manifest.xml
pkg/trunk/util/new_robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/test_filter.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -93,7 +93,7 @@
public:
- KinematicStateMonitor(void) : m_tf(*ros::Node::instance(), true, ros::Duration(1))
+ KinematicStateMonitor(void) : m_tf(*ros::Node::instance(), true, ros::Duration(10))
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -39,6 +39,7 @@
#include "planning_models/kinematic.h"
#include <ros/ros.h>
+#include <ros/node.h>
#include <boost/shared_ptr.hpp>
#include <map>
@@ -81,8 +82,9 @@
};
- RobotModels(const std::string &description) : description_(description)
+ RobotModels(const std::string &description)
{
+ description_ = nh_.getNode()->mapName(description);
loadRobot();
}
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-06 05:39:44 UTC (rev 16808)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-06 06:08:33 UTC (rev 16809)
@@ -51,7 +51,6 @@
kmodel_->setVerbose(false);
kmodel_->build(*urdf_, planning_groups_);
-
// make sure the kinematic model is in its own frame
// (remove all transforms caused by planar or floating
// joints)
Added: pkg/trunk/util/new_robot_self_filter/.build_version
===================================================================
--- pkg/trunk/util/new_robot_self_filter/.build_version (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/.build_version 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1 @@
+https://ros.svn.sf.net/svnroot/personalrobots/pkg/trunk/util/new_robot_self_filter:0
Added: pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,31 @@
+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(new_robot_self_filter)
+
+#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(test_filter test_filter.cpp)
+
Added: pkg/trunk/util/new_robot_self_filter/Makefile
===================================================================
--- pkg/trunk/util/new_robot_self_filter/Makefile (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/Makefile 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/util/new_robot_self_filter/mainpage.dox
===================================================================
--- pkg/trunk/util/new_robot_self_filter/mainpage.dox (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/mainpage.dox 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,119 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b new_robot_self_filter is ...
+
+<!--
+In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+-->
+
+
+\section codeapi Code API
+
+<!--
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group.
+-->
+
+\section rosapi ROS API
+
+<!--
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code. There is a template below that you can
+use to document each node separately.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+-->
+
+<!-- START: copy from here to 'END' for each node
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+END: copy for each node -->
+
+
+<!-- START: Uncomment if you have any command-line tools
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+
+END: Command-Line Tools Section -->
+
+*/
\ No newline at end of file
Added: pkg/trunk/util/new_robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/util/new_robot_self_filter/manifest.xml (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/manifest.xml 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,22 @@
+<package>
+ <description brief="new_robot_self_filter">
+
+ new_robot_self_filter
+
+ </description>
+ <author>Ioan Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/new_robot_self_filter</url>
+
+ <depend package="roscpp"/>
+ <depend package="tf"/>
+ <depend package="filters"/>
+ <depend package="robot_msgs"/>
+ <depend package="visualization_msgs"/>
+ <depend package="collision_space"/>
+ <depend package="planning_environment"/>
+
+</package>
+
+
Added: pkg/trunk/util/new_robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_see_filter.h (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,213 @@
+/*
+ * 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, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FILTERS_SELF_SEE_H_
+#define FILTERS_SELF_SEE_H_
+
+#include <filters/filter_base.h>
+#include <ros/console.h>
+#include <robot_msgs/PointCloud.h>
+#include <planning_environment/robot_models.h>
+#include <collision_space/point_inclusion.h>
+#include <tf/transform_listener.h>
+#include <string>
+#include <algorithm>
+
+namespace filters
+{
+
+/** \brief A filter to remove parts of the robot seen in a pointcloud
+ *
+ */
+
+template <typename T>
+class SelfFilter: public FilterBase <T>
+{
+
+protected:
+
+ struct SeeLink
+ {
+ std::string name;
+ collision_space::bodies::Body* body;
+ btTransform constTransf;
+ };
+
+ struct SortBodies
+ {
+ bool operator()(const SeeLink &b1, const SeeLink &b2)
+ {
+ return b1.body->computeVolume() > b2.body->computeVolume();
+ }
+ };
+
+public:
+ /** \brief Construct the filter */
+ SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
+ {
+ tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
+ }
+
+
+ /** \brief Destructor to clean up
+ */
+ ~SelfFilter(void)
+ {
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ if (bodies_[i].body)
+ delete bodies_[i].body;
+ }
+
+ virtual bool configure(void)
+ {
+ std::vector<std::string> links = rm_.getSelfSeeLinks();
+ double scale = rm_.getSelfSeeScale();
+ double padd = rm_.getSelfSeePadding();
+
+ // from the geometric model, find the shape of each link of interest
+ // and create a body from it, one that knows about poses and can
+ // check for point inclusion
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ SeeLink sl;
+ sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
+ if (sl.body)
+ {
+ sl.name = links[i];
+
+ // collision models may have an offset, in addition to what TF gives
+ // so we keep it around
+ sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
+ sl.body->setScale(scale);
+ sl.body->setPadding(padd);
+ bodies_.push_back(sl);
+ }
+ else
+ ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+ }
+
+ if (bodies_.empty())
+ ROS_WARN("No robot links will be checked for self collision");
+
+ // put larger volume bodies first -- higher chances of containing a point
+ std::sort(bodies_.begin(), bodies_.end(), SortBodies());
+
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
+
+ return true;
+ }
+
+
+ /** \brief Update the filter and return the data seperately
+ * \param data_in T array with length width
+ * \param data_out T array with length width
+ */
+ virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
+ {
+ if (bodies_.empty())
+ data_out = data_in;
+ else
+ {
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+
+ // place the links in the frame of the pointcloud
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ // find the transform between the link's frame and the pointcloud frame
+ tf::Stamped<btTransform> transf;
+ tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
+
+ // set it for each body; we also include the offset specified in URDF
+ bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ }
+
+ // we now decide which points we keep
+ std::vector<bool> keep(np);
+
+#pragma omp parallel for
+ for (unsigned int i = 0 ; i < np ; ++i)
+ {
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ bool out = true;
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ out = !bodies_[j].body->containsPoint(pt);
+ keep[i] = out;
+ }
+
+
+ // fill in output data
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ {
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
+ }
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i])
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+ return true;
+ }
+
+ virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
+ {
+ data_out.resize(data_in.size());
+ for (unsigned int i = 0 ; i < data_in.size() ; ++i)
+ update(data_in[i], data_out[i]);
+ return true;
+ }
+
+protected:
+
+ planning_environment::RobotModels rm_;
+
+ tf::TransformListener tf_;
+ std::vector<SeeLink> bodies_;
+
+};
+
+typedef robot_msgs::PointCloud robot_msgs_PointCloud;
+FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+
+}
+
+#endif //#ifndef FILTERS_SELF_SEE_H_
Added: pkg/trunk/util/new_robot_self_filter/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/test_filter.cpp (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/test_filter.cpp 2009-06-06 06:08:33 UTC (rev 16809)
@@ -0,0 +1,76 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include <ros/ros.h>
+#include "self_see_filter.h"
+
+class test
+{
+public:
+ test(void)
+ {
+ sf.configure();
+ sb = nh.subscribe("tilt_scan_filtered", 1, &test::myfilter, this);
+ pub = nh.advertise<robot_msgs::PointCloud>("my_point_cloud2", 10);
+
+ }
+
+ void myfilter(const robot_msgs::PointCloudConstPtr &pc)
+ {
+ robot_msgs::PointCloud out;
+ sf.update(*pc, out);
+ pub.publish(out);
+ }
+
+ ros::Subscriber sb;
+ ros::Publisher pub;
+ ros::NodeHandle nh;
+ filters::SelfFilter<robot_msgs::PointCloud> sf;
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "ttt");
+ test t;
+ ros::spin();
+
+
+ return 0;
+}
+
+
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-06 21:10:51
|
Revision: 16813
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16813&view=rev
Author: isucan
Date: 2009-06-06 21:09:43 +0000 (Sat, 06 Jun 2009)
Log Message:
-----------
optimized self filter + more thorough tests
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
pkg/trunk/util/new_robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/test_filter.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/test/test_point_inclusion.cpp
pkg/trunk/world_models/test_collision_space/src/test_cs_point_inclusion.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/planning_environment.launch
pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp
Removed Paths:
-------------
pkg/trunk/util/new_robot_self_filter/.build_version
pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h
pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp
Modified: pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/collision_checks.yaml 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/motion_planning/planning_environment/collision_checks.yaml 2009-06-06 21:09:43 UTC (rev 16813)
@@ -53,10 +53,14 @@
## list of links that the robot can see with its sensors (used to remove
## parts of the robot from scanned data)
self_see:
+ r_upper_arm_link
+ l_upper_arm_link
l_upper_arm_roll_link
r_upper_arm_roll_link
l_elbow_flex_link
r_elbow_flex_link
+ r_forearm_link
+ l_forearm_link
l_forearm_roll_link
r_forearm_roll_link
l_wrist_flex_link
@@ -64,9 +68,13 @@
l_wrist_roll_link
r_wrist_roll_link
l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
r_shoulder_pan_link
r_shoulder_lift_link
l_shoulder_pan_link
@@ -76,7 +84,7 @@
## The padding to be added for the body parts the robot can see
-self_see_padd: 0.1
+self_see_padd: 0.0
## The scaling to be considered for the body parts the robot can see
self_see_scale: 1.0
Added: pkg/trunk/motion_planning/planning_environment/planning_environment.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/planning_environment.launch (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/planning_environment.launch 2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,8 @@
+
+<launch>
+
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+ <rosparam command="load" ns="robotdesc/pr2_collision" file="$(find planning_environment)/collision_checks.yaml" />
+
+</launch>
Deleted: pkg/trunk/util/new_robot_self_filter/.build_version
===================================================================
--- pkg/trunk/util/new_robot_self_filter/.build_version 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/.build_version 2009-06-06 21:09:43 UTC (rev 16813)
@@ -1 +0,0 @@
-https://ros.svn.sf.net/svnroot/personalrobots/pkg/trunk/util/new_robot_self_filter:0
Modified: pkg/trunk/util/new_robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-06 21:09:43 UTC (rev 16813)
@@ -34,7 +34,7 @@
#include <ros/console.h>
#include <robot_msgs/PointCloud.h>
#include <planning_environment/robot_models.h>
-#include <collision_space/point_inclusion.h>
+#include <collision_space/bodies.h>
#include <tf/transform_listener.h>
#include <string>
#include <algorithm>
@@ -71,7 +71,7 @@
/** \brief Construct the filter */
SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
{
- tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
+ tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
}
@@ -86,6 +86,10 @@
virtual bool configure(void)
{
+ // keep only the points that are outside of the robot
+ // for testing purposes this may be changed to true
+ invert_ = false;
+
std::vector<std::string> links = rm_.getSelfSeeLinks();
double scale = rm_.getSelfSeeScale();
double padd = rm_.getSelfSeePadding();
@@ -120,7 +124,7 @@
for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
-
+
return true;
}
@@ -137,6 +141,7 @@
{
const unsigned int bs = bodies_.size();
const unsigned int np = data_in.pts.size();
+ std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
// place the links in the frame of the pointcloud
for (unsigned int i = 0 ; i < bs ; ++i)
@@ -147,8 +152,13 @@
// set it for each body; we also include the offset specified in URDF
bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ bodies_[i].body->computeBoundingSphere(bspheres[i]);
}
+ // compute a sphere that bounds the entire robot
+ collision_space::bodies::BoundingSphere bound;
+ collision_space::bodies::mergeBoundingSpheres(bspheres, bound);
+
// we now decide which points we keep
std::vector<bool> keep(np);
@@ -157,9 +167,11 @@
{
btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
bool out = true;
- for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
- keep[i] = out;
+ if (bound.center.distance(pt) < bound.radius)
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ out = !bodies_[j].body->containsPoint(pt);
+
+ keep[i] = invert_ ? !out : out;
}
@@ -199,10 +211,11 @@
protected:
planning_environment::RobotModels rm_;
-
+ ros::NodeHandle nh_;
tf::TransformListener tf_;
std::vector<SeeLink> bodies_;
-
+ bool invert_;
+
};
typedef robot_msgs::PointCloud robot_msgs_PointCloud;
Modified: pkg/trunk/util/new_robot_self_filter/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/test_filter.cpp 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/util/new_robot_self_filter/test_filter.cpp 2009-06-06 21:09:43 UTC (rev 16813)
@@ -36,39 +36,95 @@
#include <ros/ros.h>
#include "self_see_filter.h"
+#include <visualization_msgs/Marker.h>
-class test
+class TestSelfFilter
{
public:
- test(void)
+
+ TestSelfFilter(void)
{
- sf.configure();
- sb = nh.subscribe("tilt_scan_filtered", 1, &test::myfilter, this);
- pub = nh.advertise<robot_msgs::PointCloud>("my_point_cloud2", 10);
-
+ id_ = 1;
+ sf_.configure();
+ vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
}
- void myfilter(const robot_msgs::PointCloudConstPtr &pc)
+ void sendPoint(double x, double y, double z)
{
+ visualization_msgs::Marker mk;
+
+ mk.header.stamp = ros::Time::now();
+
+ mk.header.frame_id = "base_link";
+
+ mk.ns = "test_self_filter";
+ mk.id = id_++;
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = x;
+ mk.pose.position.y = y;
+ mk.pose.position.z = z;
+ mk.pose.orientation.w = 1.0;
+
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+
+ mk.color.a = 1.0;
+ mk.color.r = 1.0;
+ mk.color.g = 0.04;
+ mk.color.b = 0.04;
+
+ vmPub_.publish(mk);
+ }
+ void run(void)
+ {
+ robot_msgs::PointCloud in;
robot_msgs::PointCloud out;
- sf.update(*pc, out);
- pub.publish(out);
+
+ in.header.stamp = ros::Time::now();
+ in.header.frame_id = "base_link";
+
+ const unsigned int N = 100000;
+ in.pts.resize(N);
+ for (unsigned int i = 0 ; i < N ; ++i)
+ {
+ in.pts[i].x = uniform(1);
+ in.pts[i].y = uniform(1);
+ in.pts[i].z = uniform(1);
+ }
+
+ ros::WallTime tm = ros::WallTime::now();
+ sf_.update(in, out);
+ printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
+
+ for (unsigned int i = 0 ; i < out.pts.size() ; ++i)
+ {
+ sendPoint(out.pts[i].x, out.pts[i].y, out.pts[i].z);
+ }
+
+ ros::spin();
}
+protected:
+
+ double uniform(double magnitude)
+ {
+ return (2.0 * drand48() - 1.0) * magnitude;
+ }
- ros::Subscriber sb;
- ros::Publisher pub;
- ros::NodeHandle nh;
- filters::SelfFilter<robot_msgs::PointCloud> sf;
+ ros::Publisher vmPub_;
+ ros::NodeHandle nodeHandle_;
+ filters::SelfFilter<robot_msgs::PointCloud> sf_;
+ int id_;
};
int main(int argc, char **argv)
{
- ros::init(argc, argv, "ttt");
- test t;
- ros::spin();
-
+ ros::init(argc, argv, "test_self_filter");
+ TestSelfFilter t;
+ sleep(1);
+ t.run();
+
return 0;
}
Modified: pkg/trunk/world_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/CMakeLists.txt 2009-06-06 21:09:43 UTC (rev 16813)
@@ -7,7 +7,7 @@
set(CMAKE_BUILD_TYPE Release)
rospack_add_library(collision_space src/collision_space/output.cpp
- src/collision_space/point_inclusion.cpp
+ src/collision_space/bodies.cpp
src/collision_space/environment.cpp
src/collision_space/environmentODE.cpp)
rospack_link_boost(collision_space thread)
Copied: pkg/trunk/world_models/collision_space/include/collision_space/bodies.h (from rev 16812, pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h)
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/bodies.h (rev 0)
+++ pkg/trunk/world_models/collision_space/include/collision_space/bodies.h 2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,320 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#ifndef COLLISION_SPACE_POINT_INCLUSION_
+#define COLLISION_SPACE_POINT_INCLUSION_
+
+#include <planning_models/shapes.h>
+#include <LinearMath/btTransform.h>
+#include <cstdlib>
+#include <vector>
+
+/**
+ This set of classes allows quickly detecting whether a given point
+ is inside an object or not. Only basic (simple) types of objects
+ are supported: spheres, cylinders, boxes. This capability is useful
+ when removing points from inside the robot (when the robot sees its
+ arms, for example).
+*/
+
+namespace collision_space
+{
+
+ namespace bodies
+ {
+
+ struct BoundingSphere
+ {
+ btVector3 center;
+ double radius;
+ };
+
+ /** A body is a shape + its pose. Point inclusion can be
+ tested, volumes and bounding spheres can be computed.*/
+ class Body
+ {
+ public:
+
+ Body(void)
+ {
+ m_scale = 1.0;
+ m_padding = 0.0;
+ m_pose.setIdentity();
+ }
+
+ virtual ~Body(void)
+ {
+ }
+
+ /** If the dimension of the body should be scaled, this
+ method sets the scale. Default is 1.0 */
+ void setScale(double scale)
+ {
+ m_scale = scale;
+ updateInternalData();
+ }
+
+ /** Retrieve the current scale */
+ double getScale(void) const
+ {
+ return m_scale;
+ }
+
+ /** If constant padding should be added to the body, this
+ method sets the padding. Default is 0.0 */
+ void setPadding(double padd)
+ {
+ m_padding = padd;
+ updateInternalData();
+ }
+
+ /** Retrieve the current padding */
+ double getPadding(void) const
+ {
+ return m_padding;
+ }
+
+ /** Set the pose of the body. Default is identity */
+ void setPose(const btTransform &pose)
+ {
+ m_pose = pose;
+ updateInternalData();
+ }
+
+ /** Retrieve the pose of the body */
+ const btTransform& getPose(void) const
+ {
+ return m_pose;
+ }
+
+ /** Set the dimensions of the body (from corresponding shape) */
+ void setDimensions(const planning_models::shapes::Shape *shape)
+ {
+ useDimensions(shape);
+ updateInternalData();
+ }
+
+ /** Check is a point is inside the body */
+ bool containsPoint(double x, double y, double z) const
+ {
+ return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
+ }
+
+ /** Check is a point is inside the body */
+ virtual bool containsPoint(const btVector3 &p) const = 0;
+
+ /** Compute the volume of the body. This method includes
+ changes induced by scaling and padding */
+ virtual double computeVolume(void) const = 0;
+
+ /** Compute the bounding radius for the body, in its current
+ pose. Scaling and padding are accounted for. */
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
+
+ protected:
+
+ virtual void updateInternalData(void) = 0;
+ virtual void useDimensions(const planning_models::shapes::Shape *shape) = 0;
+
+ btTransform m_pose;
+ double m_scale;
+ double m_padding;
+ };
+
+ class Sphere : public Body
+ {
+ public:
+ Sphere(void) : Body()
+ {
+ m_radius = 0.0;
+ }
+
+ Sphere(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Sphere(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ double m_radius;
+ double m_radiusU;
+ double m_radius2;
+ };
+
+ class Cylinder : public Body
+ {
+ public:
+ Cylinder(void) : Body()
+ {
+ m_length = m_radius = 0.0;
+ }
+
+ Cylinder(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Cylinder(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ btVector3 m_normalH;
+ btVector3 m_normalB1;
+ btVector3 m_normalB2;
+
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radiusU;
+ double m_radiusB;
+ double m_radius2;
+ };
+
+
+ class Box : public Body
+ {
+ public:
+ Box(void) : Body()
+ {
+ m_length = m_width = m_height = 0.0;
+ }
+
+ Box(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Box(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape); // (x, y, z) = (length, width, height)
+ virtual void updateInternalData(void);
+
+ btVector3 m_center;
+ btVector3 m_normalL;
+ btVector3 m_normalW;
+ btVector3 m_normalH;
+
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
+ double m_radiusB;
+ };
+
+
+ class ConvexMesh : public Body
+ {
+ public:
+
+ ConvexMesh(void) : Body()
+ {
+ m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+ }
+
+ ConvexMesh(const planning_models::shapes::Shape *shape) : Body()
+ {
+ m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
+ setDimensions(shape);
+ }
+
+ virtual ~ConvexMesh(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const;
+ virtual double computeVolume(void) const;
+ virtual void computeBoundingSphere(BoundingSphere &sphere) const;
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape);
+ virtual void updateInternalData(void);
+
+ unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
+ bool isPointInsidePlanes(const btVector3& point) const;
+
+ std::vector<btVector4> m_planes;
+ std::vector<btVector3> m_vertices;
+ std::vector<unsigned int> m_triangles;
+ btTransform m_iPose;
+
+ btVector3 m_center;
+ btVector3 m_meshCenter;
+ double m_radiusB;
+ };
+
+
+ /** Create a body from a given shape */
+ Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
+
+ /** Compute a bounding sphere to enclose a set of bounding spheres */
+ void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
+ }
+}
+
+#endif
Property changes on: pkg/trunk/world_models/collision_space/include/collision_space/bodies.h
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h 2009-06-06 17:46:09 UTC (rev 16812)
+++ pkg/trunk/world_models/collision_space/include/collision_space/point_inclusion.h 2009-06-06 21:09:43 UTC (rev 16813)
@@ -1,311 +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 Ioan Sucan */
-
-#ifndef COLLISION_SPACE_POINT_INCLUSION_
-#define COLLISION_SPACE_POINT_INCLUSION_
-
-#include <planning_models/shapes.h>
-#include <LinearMath/btTransform.h>
-#include <cstdlib>
-#include <vector>
-
-/**
- This set of classes allows quickly detecting whether a given point
- is inside an object or not. Only basic (simple) types of objects
- are supported: spheres, cylinders, boxes. This capability is useful
- when removing points from inside the robot (when the robot sees its
- arms, for example).
-*/
-
-namespace collision_space
-{
-
- namespace bodies
- {
-
- /** A body is a shape + its pose. Point inclusion can be
- tested, volumes and bounding spheres can be computed.*/
- class Body
- {
- public:
-
- Body(void)
- {
- m_scale = 1.0;
- m_padding = 0.0;
- m_pose.setIdentity();
- }
-
- virtual ~Body(void)
- {
- }
-
- /** If the dimension of the body should be scaled, this
- method sets the scale. Default is 1.0 */
- void setScale(double scale)
- {
- m_scale = scale;
- updateInternalData();
- }
-
- /** Retrieve the current scale */
- double getScale(void) const
- {
- return m_scale;
- }
-
- /** If constant padding should be added to the body, this
- method sets the padding. Default is 0.0 */
- void setPadding(double padd)
- {
- m_padding = padd;
- updateInternalData();
- }
-
- /** Retrieve the current padding */
- double getPadding(void) const
- {
- return m_padding;
- }
-
- /** Set the pose of the body. Default is identity */
- void setPose(const btTransform &pose)
- {
- m_pose = pose;
- updateInternalData();
- }
-
- /** Retrieve the pose of the body */
- const btTransform& getPose(void) const
- {
- return m_pose;
- }
-
- /** Set the dimensions of the body (from corresponding shape) */
- void setDimensions(const planning_models::shapes::Shape *shape)
- {
- useDimensions(shape);
- updateInternalData();
- }
-
- /** Check is a point is inside the body */
- bool containsPoint(double x, double y, double z) const
- {
- return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
- }
-
- /** Check is a point is inside the body */
- virtual bool containsPoint(const btVector3 &p) const = 0;
-
- /** Compute the volume of the body. This method includes
- changes induced by scaling and padding */
- virtual double computeVolume(void) const = 0;
-
- /** Compute the bounding radius for the body, in its current
- pose. Scaling and padding are accounted for. */
- virtual void computeBoundingSphere(btVector3 ¢er, double &radius) const = 0;
-
- protected:
-
- virtual void updateInternalData(void) = 0;
- virtual void useDimensions(const planning_models::shapes::Shape *shape) = 0;
-
- btTransform m_pose;
- double m_scale;
- double m_padding;
- };
-
- class Sphere : public Body
- {
- public:
- Sphere(void) : Body()
- {
- m_radius = 0.0;
- }
-
- Sphere(const planning_models::shapes::Shape *shape) : Body()
- {
- setDimensions(shape);
- }
-
- virtual ~Sphere(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const;
- virtual double computeVolume(void) const;
- virtual void computeBoundingSphere(btVector3 ¢er, double &radius) const;
-
- protected:
-
- virtual void useDimensions(const planning_models::shapes::Shape *shape);
- virtual void updateInternalData(void);
-
- btVector3 m_center;
- double m_radius;
- double m_radiusU;
- double m_radius2;
- };
-
- class Cylinder : public Body
- {
- public:
- Cylinder(void) : Body()
- {
- m_length = m_radius = 0.0;
- }
-
- Cylinder(const planning_models::shapes::Shape *shape) : Body()
- {
- setDimensions(shape);
- }
-
- virtual ~Cylinder(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const;
- virtual double computeVolume(void) const;
- virtual void computeBoundingSphere(btVector3 ¢er, double &radius) const;
-
- protected:
-
- virtual void useDimensions(const planning_models::shapes::Shape *shape);
- virtual void updateInternalData(void);
-
- btVector3 m_center;
- btVector3 m_normalH;
- btVector3 m_normalB1;
- btVector3 m_normalB2;
-
- double m_length;
- double m_length2;
- double m_radius;
- double m_radiusU;
- double m_radiusB;
- double m_radius2;
- };
-
-
- class Box : public Body
- {
- public:
- Box(void) : Body()
- {
- m_length = m_width = m_height = 0.0;
- }
-
- Box(const planning_models::shapes::Shape *shape) : Body()
- {
- setDimensions(shape);
- }
-
- virtual ~Box(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const;
- virtual double computeVolume(void) const;
- virtual void computeBoundingSphere(btVector3 ¢er, double &radius) const;
-
- protected:
-
- virtual void useDimensions(const planning_models::shapes::Shape *shape); // (x, y, z) = (length, width, height)
- virtual void updateInternalData(void);
-
- btVector3 m_center;
- btVector3 m_normalL;
- btVector3 m_normalW;
- btVector3 m_normalH;
-
- double m_length;
- double m_width;
- double m_height;
- double m_length2;
- double m_width2;
- double m_height2;
- double m_radiusB;
- };
-
-
- class ConvexMesh : public Body
- {
- public:
-
- ConvexMesh(void) : Body()
- {
- m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
- }
-
- ConvexMesh(const planning_models::shapes::Shape *shape) : Body()
- {
- m_meshCenter.setValue(btScalar(0), btScalar(0), btScalar(0));
- setDimensions(shape);
- }
-
- virtual ~ConvexMesh(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const;
- virtual double computeVolume(void) const;
- virtual void computeBoundingSphere(btVector3 ¢er, double &radius) const;
-
- protected:
-
- virtual void useDimensions(const planning_models::shapes::Shape *shape);
- virtual void updateInternalData(void);
-
- unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
- bool isPointInsidePlanes(const btVector3& point) const;
-
- std::vector<btVector4> m_planes;
- std::vector<btVector3> m_vertices;
- std::vector<unsigned int> m_triangles;
- btTransform m_iPose;
-
- btVector3 m_center;
- btVector3 m_meshCenter;
- double m_radiusB;
- };
-
-
- Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
-
- }
-}
-
-#endif
Copied: pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp (from rev 16812, pkg/trunk/world_models/collision_space/src/collision_space/point_inclusion.cpp)
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp (rev 0)
+++ pkg/trunk/world_models/collision_space/src/collision_space/bodies.cpp 2009-06-06 21:09:43 UTC (rev 16813)
@@ -0,0 +1,383 @@
+/*********************************************************************
+* 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:
+*
+* ...
[truncated message content] |
|
From: <is...@us...> - 2009-06-07 00:04:56
|
Revision: 16815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16815&view=rev
Author: isucan
Date: 2009-06-07 00:04:10 +0000 (Sun, 07 Jun 2009)
Log Message:
-----------
removed support for multiple robots from collision space since the planning model can support that as well
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequest.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -290,7 +290,7 @@
{
/* set the pose of the whole robot */
psetup->model->kmodel->computeTransforms(&start_state.vals[0]);
- psetup->model->collisionSpace->updateRobotModel(psetup->model->collisionSpaceID);
+ psetup->model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -126,7 +126,7 @@
void update(const ompl::sb::State *state) const
{
m_model->kmodel->computeTransformsGroup(state->values, m_model->groupID);
- m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+ m_model->collisionSpace->updateRobotModel();
}
mutable RKPModelBase *m_model;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPModelBase.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -51,8 +51,7 @@
public:
RKPModelBase(void)
{
- groupID = -1;
- collisionSpaceID = 0;
+ groupID = -1;
}
virtual ~RKPModelBase(void)
@@ -61,7 +60,6 @@
boost::mutex lock;
boost::shared_ptr<collision_space::EnvironmentModel> collisionSpace;
- unsigned int collisionSpaceID;
boost::shared_ptr<planning_models::KinematicModel> kmodel;
std::string groupName;
int groupID;
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -59,9 +59,9 @@
{
m_model->kmodel->lock();
m_model->kmodel->computeTransformsGroup(static_cast<const ompl::sb::State*>(state)->values, m_model->groupID);
- m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+ m_model->collisionSpace->updateRobotModel();
- bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
+ bool valid = !m_model->collisionSpace->isCollision();
if (valid)
valid = m_kce.decide();
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/CollisionSpaceMonitor.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -54,8 +54,7 @@
void kinematic_planning::CollisionSpaceMonitor::attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject)
{
m_collisionSpace->lock();
- int model_id = m_collisionSpace->getModelID(attachedObject->robot_name);
- planning_models::KinematicModel::Link *link = model_id >= 0 ? m_kmodel->getLink(attachedObject->link_name) : NULL;
+ planning_models::KinematicModel::Link *link = m_kmodel->getLink(attachedObject->link_name);
if (link)
{
@@ -89,7 +88,7 @@
}
// update the collision model
- m_collisionSpace->updateAttachedBodies(model_id);
+ m_collisionSpace->updateAttachedBodies();
ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
}
else
@@ -102,11 +101,7 @@
bool kinematic_planning::CollisionSpaceMonitor::setCollisionState(motion_planning_srvs::CollisionCheckState::Request &req, motion_planning_srvs::CollisionCheckState::Response &res)
{
m_collisionSpace->lock();
- int model_id = m_collisionSpace->getModelID(req.robot_name);
- if (model_id >= 0)
- res.value = m_collisionSpace->setCollisionCheck(model_id, req.link_name, req.value ? true : false);
- else
- res.value = -1;
+ res.value = m_collisionSpace->setCollisionCheck(req.link_name, req.value ? true : false);
m_collisionSpace->unlock();
if (res.value == -1)
ROS_WARN("Unable to change collision checking state for link '%s' on '%s'", req.link_name.c_str(), req.robot_name.c_str());
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -499,7 +499,6 @@
/* set the data for the model */
RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupName = m_kmodel->getModelName();
@@ -515,7 +514,6 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
RKPModel *model = new RKPModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_validator.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -163,7 +163,7 @@
{
/* set the pose of the whole robot to the current state */
model->kmodel->computeTransforms(m_robotState->getParams());
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
@@ -229,7 +229,7 @@
{
/* set the pose of the whole robot */
model->kmodel->computeTransforms(&req.start_state.vals[0]);
- model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+ model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
@@ -281,7 +281,6 @@
/* set the data for the model */
myModel *model = new myModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupName = m_kmodel->getModelName();
@@ -297,7 +296,6 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
myModel *model = new myModel();
- model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -127,12 +127,12 @@
{
CollisionSpaceMonitor::stateUpdate();
- if (m_collisionSpace && m_collisionSpace->getModelCount() == 1)
+ if (m_collisionSpace)
{
m_collisionSpace->lock();
m_kmodel->computeTransforms(m_robotState->getParams());
- m_collisionSpace->updateRobotModel(0);
- bool invalid = m_collisionSpace->isCollision(0);
+ m_collisionSpace->updateRobotModel();
+ bool invalid = m_collisionSpace->isCollision();
m_collisionSpace->unlock();
std_msgs::Byte msg;
msg.data = invalid ? 0 : 1;
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -44,10 +44,10 @@
if (urdf_.get() && kmodel_.get())
{
ode_collision_model_->lock();
- unsigned int cid = ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
+ ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
- ode_collision_model_->addSelfCollisionGroup(cid, self_collision_check_groups_[i]);
- ode_collision_model_->updateRobotModel(cid);
+ ode_collision_model_->addSelfCollisionGroup(self_collision_check_groups_[i]);
+ ode_collision_model_->updateRobotModel();
ode_collision_model_->unlock();
}
}
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -155,11 +155,11 @@
m_kmodel->computeTransforms(m_robotState->getParams());
// ask the collision space to look at the updates
- m_collisionSpace->updateRobotModel(0);
+ m_collisionSpace->updateRobotModel();
// get the first contact point
std::vector<collision_space::EnvironmentModel::Contact> contacts;
- m_collisionSpace->getCollisionContacts(0, contacts, 1);
+ m_collisionSpace->getCollisionContacts(contacts, 1);
if (contacts.size() > 0)
{
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -53,10 +53,9 @@
the base (abstract) definition. Different implementations are
possible. The class is aware of a certain set of fixed
(addStatic*) obstacles that never change, a set of obstacles that
- can change (removed by clearObstacles()) and a set of kinematic
- robots. The class provides functionality for checking whether a
+ can change (removed by clearObstacles()) and a kinematic
+ robot model. The class provides functionality for checking whether a
given robot is in collision.
-
*/
@@ -99,10 +98,10 @@
bool getSelfCollision(void) const;
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
+ virtual void addSelfCollisionGroup(std::vector<std::string> &links) = 0;
/** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
- virtual int setCollisionCheck(unsigned int model_id, const std::string &link, bool state) = 0;
+ virtual int setCollisionCheck(const std::string &link, bool state) = 0;
/** Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
@@ -110,34 +109,28 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual unsigned int addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id) = 0;
+ virtual void updateRobotModel(void) = 0;
/** Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies(unsigned int model_id) = 0;
+ virtual void updateAttachedBodies(void) = 0;
- /** Get the number of loaded models */
- unsigned int getModelCount(void) const;
-
- /** Get a specific model */
- boost::shared_ptr<planning_models::KinematicModel> getRobotModel(unsigned int model_id) const;
+ /** Get the robot model */
+ boost::shared_ptr<planning_models::KinematicModel> getRobotModel(void) const;
- /** Get the model ID based on the model (robot) name; returns -1 if model not found. */
- int getModelID(const std::string& robot_name) const;
-
/**********************************************************************/
/* Collision Checking Routines */
/**********************************************************************/
- /** Check if a model is in collision */
- virtual bool isCollision(unsigned int model_id) = 0;
+ /** Check if a model is in collision. Contacts are not computed */
+ virtual bool isCollision(void) = 0;
/** Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
+ virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1) = 0;
/**********************************************************************/
@@ -172,14 +165,13 @@
protected:
- boost::mutex m_lock;
- bool m_selfCollision;
- bool m_verbose;
- msg::Interface m_msg;
+ boost::mutex m_lock;
+ bool m_selfCollision;
+ bool m_verbose;
+ msg::Interface m_msg;
/** List of loaded robot models */
- std::vector< boost::shared_ptr<planning_models::KinematicModel> >
- m_models;
+ boost::shared_ptr<planning_models::KinematicModel> m_robotModel;
};
}
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-06-07 00:04:10 UTC (rev 16815)
@@ -73,14 +73,14 @@
/** Return the space ID for the space in which static objects are added */
dSpaceID getODEBasicGeomSpace(void) const;
- /** Return the space ID for the space in which the particular model is instanciated */
- dSpaceID getModelODESpace(unsigned int model_id) const;
+ /** Return the space ID for the space in which the robot model is instanciated */
+ dSpaceID getModelODESpace(void) const;
/** Get the list of contacts (collisions) */
- virtual bool getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count = 1);
+ virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
/** Check if a model is in collision */
- virtual bool isCollision(unsigned int model_id);
+ virtual bool isCollision(void);
/** Remove all obstacles from collision model */
virtual void clearObstacles(void);
@@ -97,24 +97,24 @@
bodies (multiplicative factor). The padding can be used to
increase or decrease the robot's bodies with by an
additive term */
- virtual unsigned int addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
+ virtual void addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id);
+ virtual void updateRobotModel(void);
/** Update the set of bodies that are attached to the robot (re-creates them) */
- virtual void updateAttachedBodies(unsigned int model_id);
+ virtual void updateAttachedBodies(void);
/** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+ virtual void addSelfCollisionGroup(std::vector<std::string> &links);
/** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
- virtual int setCollisionCheck(unsigned int model_id, const std::string &link, bool state);
+ virtual int setCollisionCheck(const std::string &link, bool state);
protected:
/** Internal function for collision detection */
- void testCollision(unsigned int model_id, void *data);
+ void testCollision(void *data);
class ODECollide2
{
@@ -235,15 +235,15 @@
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
- std::vector<ModelInfo> m_modelsGeom;
- dSpaceID m_space;
- dSpaceID m_spaceBasicGeoms;
+ ModelInfo m_modelGeom;
+ dSpaceID m_space;
+ dSpaceID m_spaceBasicGeoms;
/* This is where geoms from the world (that can be cleared and recreated) are added; the space for this is m_space */
- ODECollide2 m_collide2;
+ ODECollide2 m_collide2;
/* This is where static geoms from the world (that are not cleared) are added; the space for this is m_spaceBasicGeoms */
- std::vector<dGeomID> m_basicGeoms;
+ std::vector<dGeomID> m_basicGeoms;
};
}
Modified: pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/src/collision_space/environment.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -41,11 +41,9 @@
m_verbose = verbose;
}
-unsigned int collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModel::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int pos = m_models.size();
- m_models.push_back(model);
- return pos;
+ m_robotModel = model;
}
void collision_space::EnvironmentModel::lock(void)
@@ -67,22 +65,3 @@
{
return m_selfCollision;
}
-
-
-unsigned int collision_space::EnvironmentModel::getModelCount(void) const
-{
- return m_models.size();
-}
-
-int collision_space::EnvironmentModel::getModelID(const std::string& robot_name) const
-{
- for (unsigned int i = 0 ; i < m_models.size() ; ++i)
- if (m_models[i]->getModelName() == robot_name)
- return i;
- return -1;
-}
-
-boost::shared_ptr<planning_models::KinematicModel> collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
-{
- return m_models[model_id];
-}
Modified: pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp 2009-06-06 22:49:40 UTC (rev 16814)
+++ pkg/trunk/world_models/collision_space/src/collision_space/environmentODE.cpp 2009-06-07 00:04:10 UTC (rev 16815)
@@ -42,37 +42,31 @@
void collision_space::EnvironmentModelODE::freeMemory(void)
{
- for (unsigned int i = 0 ; i < m_modelsGeom.size() ; ++i)
- {
- for (unsigned int j = 0 ; j < m_modelsGeom[i].linkGeom.size() ; ++j)
- delete m_modelsGeom[i].linkGeom[j];
- dSpaceDestroy(m_modelsGeom[i].space);
- }
+ for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
+ delete m_modelGeom.linkGeom[j];
+ if (m_modelGeom.space)
+ dSpaceDestroy(m_modelGeom.space);
if (m_space)
dSpaceDestroy(m_space);
if (m_spaceBasicGeoms)
dSpaceDestroy(m_spaceBasicGeoms);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
+void collision_space::EnvironmentModelODE::addRobotModel(const boost::shared_ptr<planning_models::KinematicModel> &model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
+ collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
- if (m_modelsGeom.size() <= id)
- {
- m_modelsGeom.resize(id + 1);
- m_modelsGeom[id].space = dHashSpaceCreate(0);
- m_modelsGeom[id].scale = scale;
- m_modelsGeom[id].padding = padding;
- }
+ m_modelGeom.space = dHashSpaceCreate(0);
+ m_modelGeom.scale = scale;
+ m_modelGeom.padding = padding;
std::map<std::string, bool> exists;
for (unsigned int i = 0 ; i < links.size() ; ++i)
exists[links[i]] = true;
- for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
+ for (unsigned int j = 0 ; j < m_robotModel->getRobotCount() ; ++j)
{
- planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(j);
+ planning_models::KinematicModel::Robot *robot = m_robotModel->getRobot(j);
for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
{
/* skip this link if we have no geometry or if the link
@@ -86,19 +80,18 @@
kGeom *kg = new kGeom();
kg->link = robot->links[i];
kg->enabled = true;
- dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale, padding);
+ dGeomID g = createODEGeom(m_modelGeom.space, robot->links[i]->shape, scale, padding);
assert(g);
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale, padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
kg->geom.push_back(ga);
}
- m_modelsGeom[id].linkGeom.push_back(kg);
+ m_modelGeom.linkGeom.push_back(kg);
}
}
- return id;
}
dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::shapes::Shape *shape, double scale, double padding) const
@@ -139,12 +132,12 @@
dGeomSetQuaternion(geom, q);
}
-void collision_space::EnvironmentModelODE::updateAttachedBodies(unsigned int model_id)
+void collision_space::EnvironmentModelODE::updateAttachedBodies(void)
{
- const unsigned int n = m_modelsGeom[model_id].linkGeom.size();
+ const unsigned int n = m_modelGeom.linkGeom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- kGeom *kg = m_modelsGeom[model_id].linkGeom[i];
+ kGeom *kg = m_modelGeom.linkGeom[i];
/* clear previosly attached bodies */
for (unsigned int k = 1 ; k < kg->geom.size() ; ++k)
@@ -155,23 +148,23 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale, m_modelsGeom[model_id].padding);
+ dGeomID ga = createODEGeom(m_modelGeom.space, kg->link->attachedBodies[k]->shape, m_modelGeom.scale, m_modelGeom.padding);
assert(ga);
kg->geom.push_back(ga);
}
}
}
-void collision_space::EnvironmentModelODE::updateRobotModel(unsigned int model_id)
+void collision_space::EnvironmentModelODE::updateRobotModel(void)
{
- const unsigned int n = m_modelsGeom[model_id].linkGeom.size();
+ const unsigned int n = m_modelGeom.linkGeom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- updateGeom(m_modelsGeom[model_id].linkGeom[i]->geom[0], m_modelsGeom[model_id].linkGeom[i]->link->globalTrans);
- const unsigned int nab = m_modelsGeom[model_id].linkGeom[i]->link->attachedBodies.size();
+ updateGeom(m_modelGeom.linkGeom[i]->geom[0], m_modelGeom.linkGeom[i]->link->globalTrans);
+ const unsigned int nab = m_modelGeom.linkGeom[i]->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
- updateGeom(m_modelsGeom[model_id].linkGeom[i]->geom[k + 1], m_modelsGeom[model_id].linkGeom[i]->link->attachedBodies[k]->globalTrans);
+ updateGeom(m_modelGeom.linkGeom[i]->geom[k + 1], m_modelGeom.linkGeom[i]->link->attachedBodies[k]->globalTrans);
}
}
@@ -301,9 +294,9 @@
return m_spaceBasicGeoms;
}
-dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
+dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(void) const
{
- return m_modelsGeom[model_id].space;
+ return m_modelGeom.space;
}
struct CollisionData
@@ -385,39 +378,39 @@
}
}
-bool collision_space::EnvironmentModelODE::getCollisionContacts(unsigned int model_id, std::vector<Contact> &contacts, unsigned int max_count)
+bool collision_space::EnvironmentModelODE::getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count)
{
CollisionData cdata;
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
- testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
-bool collision_space::EnvironmentModelODE::isCollision(unsigned int model_id)
+bool collision_space::EnvironmentModelODE::isCollision(void)
{
CollisionData cdata;
- testCollision(model_id, reinterpret_cast<void*>(&cdata));
+ testCollision(reinterpret_cast<void*>(&cdata));
return cdata.collides;
}
-void collision_space::EnvironmentModelODE::testCollision(unsigned int model_id, void *data)
+void collision_space::EnvironmentModelODE::testCollision(void *data)
{
CollisionData *cdata = reinterpret_cast<CollisionData*>(data);
/* check self collision */
if (m_selfCollision)
{
- for (int i = m_modelsGeom[model_id].selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
+ for (int i = m_modelGeom.selfCollision.size() - 1 ; i >= 0 && !cdata->done ; --i)
{
- const std::vector<unsigned int> &vec = m_modelsGeom[model_id].selfCollision[i];
+ const std::vector<unsigned int> &vec = m_modelGeom.selfCollision[i];
unsigned int n = vec.size();
for (unsigned int j = 0 ; j < n && !cdata->done ; ++j)
{
- cdata->link1 = m_modelsGeom[model_id].linkGeom[vec[j]]->link;
- const unsigned int njg = m_modelsGeom[model_id].linkGeom[vec[j]]->geom.size();
+ cdata->link1 = m_modelGeom.linkGeom[vec[j]]->link;
+ const unsigned int njg = m_modelGeom.linkGeom[vec[j]]->geom.size();
for (unsigned int k = j + 1 ; k < n && !cdata->done; ++k)
{
@@ -426,15 +419,15 @@
// get the data anyway, we attempt to speed things
// up using it.
- const unsigned int nkg = m_modelsGeom[model_id].linkGeom[vec[k]]->geom.size();
- cdata->link2 = m_modelsGeom[model_id].linkGeom[vec[k]]->link;
+ const unsigned int nkg = m_modelGeom.linkGeom[vec[k]]->geom.size();
+ cdata->link2 = m_modelGeom.linkGeom[vec[k]]->link;
...
[truncated message content] |
|
From: <is...@us...> - 2009-06-07 21:59:33
|
Revision: 16826
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16826&view=rev
Author: isucan
Date: 2009-06-07 21:59:26 +0000 (Sun, 07 Jun 2009)
Log Message:
-----------
moved configuration for pr2 collision checking & planning
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/
pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml
pkg/trunk/robot_descriptions/pr2/pr2_defs/pr2_description.launch
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
pkg/trunk/motion_planning/planning_environment/planning.yaml
pkg/trunk/motion_planning/planning_environment/planning_environment.launch
Modified: pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-07 21:59:26 UTC (rev 16826)
@@ -2,9 +2,9 @@
<launch>
<!--
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
--->
<rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
<rosparam command="load" ns="robotdesc/pr2_collision" file="$(find planning_environment)/collision_checks.yaml" />
+-->
<node pkg="ompl_planning" type="motion_planner" args="robot_description:=robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen">
<param name="refresh_interval_collision_map" type="double" value="0.0" />
<param name="refresh_interval_kinematic_state" type="double" value="0.0" />
Deleted: pkg/trunk/motion_planning/planning_environment/collision_checks.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/collision_checks.yaml 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/collision_checks.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -1,90 +0,0 @@
-## This file should be loaded under <robot description>_collision
-
-
-## links for which collision checking with the environment should be performed
-collision_links:
- base_link
- torso_lift_link
- head_pan_link
- head_tilt_link
- r_shoulder_pan_link
- r_shoulder_lift_link
- r_upper_arm_roll_link
- r_upper_arm_link
- r_elbow_flex_link
- r_forearm_roll_link
- r_forearm_link
- r_wrist_flex_link
- r_wrist_roll_link
- r_gripper_palm_link
- r_gripper_l_finger_link
- r_gripper_r_finger_link
- r_gripper_l_finger_tip_link
- r_gripper_r_finger_tip_link
-
-## self collision is performed between groups of links, usually pairs
-self_collision_groups: scg1 scg2 scg3 scg4 scg5 scg6
-
-scg1:
- r_forearm_link
- base_link
-
-scg2:
- r_gripper_palm_link
- base_link
-
-scg3:
- r_gripper_l_finger_link
- base_link
-
-scg4:
- r_gripper_r_finger_link
- base_link
-
-scg5:
- r_gripper_l_finger_tip_link
- base_link
-
-scg6:
- r_gripper_r_finger_tip_link
- base_link
-
-
-## list of links that the robot can see with its sensors (used to remove
-## parts of the robot from scanned data)
-self_see:
- r_upper_arm_link
- l_upper_arm_link
- l_upper_arm_roll_link
- r_upper_arm_roll_link
- l_elbow_flex_link
- r_elbow_flex_link
- r_forearm_link
- l_forearm_link
- l_forearm_roll_link
- r_forearm_roll_link
- l_wrist_flex_link
- r_wrist_flex_link
- l_wrist_roll_link
- r_wrist_roll_link
- l_gripper_l_finger_link
- l_gripper_l_finger_tip_link
- l_gripper_r_finger_link
- l_gripper_r_finger_tip_link
- r_gripper_l_finger_link
- r_gripper_l_finger_tip_link
- r_gripper_r_finger_link
- r_gripper_r_finger_tip_link
- r_shoulder_pan_link
- r_shoulder_lift_link
- l_shoulder_pan_link
- l_shoulder_lift_link
- torso_lift_link
- base_link
-
-
-## The padding to be added for the body parts the robot can see
-self_see_padd: 0.0
-
-## The scaling to be considered for the body parts the robot can see
-self_see_scale: 1.0
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-07 21:59:26 UTC (rev 16826)
@@ -124,7 +124,7 @@
return self_see_links_;
}
- const std::vector< std::vector<std::string> > &getSelfCollisionGroups(void) const
+ const std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &getSelfCollisionGroups(void) const
{
return self_collision_check_groups_;
}
@@ -153,7 +153,7 @@
void getSelfSeeLinks(std::vector<std::string> &links);
void getCollisionCheckLinks(std::vector<std::string> &links);
- void getSelfCollisionGroups(std::vector< std::vector<std::string> > &groups);
+ void getSelfCollisionGroups(std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &groups);
ros::NodeHandle nh_;
@@ -166,7 +166,8 @@
std::map< std::string, std::vector<std::string> > planning_groups_;
std::vector<std::string> self_see_links_;
std::vector<std::string> collision_check_links_;
- std::vector< std::vector<std::string> > self_collision_check_groups_;
+ std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > >
+ self_collision_check_groups_;
};
Deleted: pkg/trunk/motion_planning/planning_environment/planning.yaml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/planning.yaml 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/planning.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -1,74 +0,0 @@
-## This file should be loaded under <robot description>_planning
-
-## the list of groups for which motion planning can be performed
-group_list:
- base
- left_arm
- right_arm
- arms
- base_left_arm
- base_right_arm
- base_arms
-
-## the definition of each group
-groups:
-
- base:
- links:
- base_link
- planner_configs:
- RRTConfig1 SBLConfig1
-
- left_arm:
- links:
- l_shoulder_pan_link
- l_shoulder_lift_link
- l_upper_arm_roll_link
- l_upper_arm_link
- l_elbow_flex_link
- l_forearm_roll_link
- l_forearm_link
- l_wrist_flex_link
- l_wrist_roll_link
- planner_configs:
- RRTConfig2 SBLConfig2
-
- right_arm:
- links:
- r_shoulder_pan_link
- r_shoulder_lift_link
- r_upper_arm_roll_link
- r_upper_arm_link
- r_elbow_flex_link
- r_forearm_roll_link
- r_forearm_link
- r_wrist_flex_link
- r_wrist_roll_link
- planner_configs:
- RRTConfig2 SBLConfig2
-
-## the planner configurations; each config must have a type, which specifies
-## the planner to be used; other parameters can be specified as well
-
-planner_configs:
-
- RRTConfig1:
- type: RRT
- range: 0.75
-
- SBLConfig1:
- type: SBL
- projection: 0 1
- celldim: 1 1
- range: 0.5
-
- RRTConfig2:
- type: RRT
- range: 0.75
-
- SBLConfig2:
- type: SBL
- projection: 5 6
- celldim: 0.1 0.1
-
-
\ No newline at end of file
Deleted: pkg/trunk/motion_planning/planning_environment/planning_environment.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/planning_environment.launch 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/planning_environment.launch 2009-06-07 21:59:26 UTC (rev 16826)
@@ -1,8 +0,0 @@
-
-<launch>
-
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
- <rosparam command="load" ns="robotdesc/pr2_collision" file="$(find planning_environment)/collision_checks.yaml" />
-
-</launch>
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-07 21:59:26 UTC (rev 16826)
@@ -45,8 +45,17 @@
{
ode_collision_model_->lock();
ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
+
+ // form all pairs of links that can collide and add them as self-collision groups
for (unsigned int i = 0 ; i < self_collision_check_groups_.size() ; ++i)
- ode_collision_model_->addSelfCollisionGroup(self_collision_check_groups_[i]);
+ for (unsigned int g1 = 0 ; g1 < self_collision_check_groups_[i].first.size() ; ++g1)
+ for (unsigned int g2 = 0 ; g2 < self_collision_check_groups_[i].second.size() ; ++g2)
+ {
+ std::vector<std::string> scg;
+ scg.push_back(self_collision_check_groups_[i].first[g1]);
+ scg.push_back(self_collision_check_groups_[i].second[g2]);
+ ode_collision_model_->addSelfCollisionGroup(scg);
+ }
ode_collision_model_->updateRobotModel();
ode_collision_model_->unlock();
}
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-07 20:12:31 UTC (rev 16825)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-07 21:59:26 UTC (rev 16826)
@@ -167,7 +167,7 @@
}
}
-void planning_environment::RobotModels::getSelfCollisionGroups(std::vector< std::vector<std::string> > &groups)
+void planning_environment::RobotModels::getSelfCollisionGroups(std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &groups)
{
std::string group_list;
nh_.param(description_ + "_collision/self_collision_groups", group_list, std::string(""));
@@ -179,21 +179,40 @@
group_list_stream >> name;
if (name.size() == 0)
continue;
- nh_.param(description_ + "_collision/" + name, group_elems, std::string(""));
- std::stringstream group_elems_stream(group_elems);
- std::vector<std::string> this_group;
- while (group_elems_stream.good() && !group_elems_stream.eof())
+
+ std::pair < std::vector<std::string>, std::vector<std::string> > this_group;
+
+ // read part 'a'
+ nh_.param(description_ + "_collision/" + name + "/a", group_elems, std::string(""));
+ std::stringstream group_elems_stream_a(group_elems);
+ while (group_elems_stream_a.good() && !group_elems_stream_a.eof())
{
std::string link_name;
- group_elems_stream >> link_name;
+ group_elems_stream_a >> link_name;
if (link_name.size() == 0)
continue;
if (urdf_->getLink(link_name))
- this_group.push_back(link_name);
+ this_group.first.push_back(link_name);
else
ROS_ERROR("Unknown link: '%s'", link_name.c_str());
}
- if (this_group.size() > 0)
+
+ // read part 'b'
+ nh_.param(description_ + "_collision/" + name + "/b", group_elems, std::string(""));
+ std::stringstream group_elems_stream_b(group_elems);
+ while (group_elems_stream_b.good() && !group_elems_stream_b.eof())
+ {
+ std::string link_name;
+ group_elems_stream_b >> link_name;
+ if (link_name.size() == 0)
+ continue;
+ if (urdf_->getLink(link_name))
+ this_group.second.push_back(link_name);
+ else
+ ROS_ERROR("Unknown link: '%s'", link_name.c_str());
+ }
+
+ if (this_group.first.size() > 0 && this_group.second.size() > 0)
groups.push_back(this_group);
}
}
Added: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_both_arms.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -0,0 +1,94 @@
+## This file should be loaded under <robot description>_collision
+
+
+## links for which collision checking with the environment should be performed
+collision_links:
+ base_link
+ torso_lift_link
+ head_pan_link
+ head_tilt_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_palm_link
+ r_gripper_l_finger_link
+ r_gripper_r_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_palm_link
+ l_gripper_l_finger_link
+ l_gripper_r_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_tip_link
+
+## self collision is performed between groups of links
+self_collision_groups: scg_r scg_l scg_lr
+
+## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_r:
+ a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
+ b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+
+## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_l:
+ a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
+ b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+
+## for arms with each other; self-collision if any link in 'a' collides with some link in 'b'
+scg_lr:
+ a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
+ b: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
+
+## list of links that the robot can see with its sensors (used to remove
+## parts of the robot from scanned data)
+self_see:
+ l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ torso_lift_link
+ base_link
+
+
+## The padding to be added for the body parts the robot can see
+self_see_padd: 0.1
+
+## The scaling to be considered for the body parts the robot can see
+self_see_scale: 1.0
Added: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_left_arm.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -0,0 +1,57 @@
+## This file should be loaded under <robot description>_collision
+
+
+## links for which collision checking with the environment should be performed
+collision_links:
+ base_link
+ torso_lift_link
+ head_pan_link
+ head_tilt_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_palm_link
+ l_gripper_l_finger_link
+ l_gripper_r_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_tip_link
+
+## self collision is performed between groups of links
+self_collision_groups: scg_l
+
+## -- for left arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_l:
+ a: l_forearm_link l_gripper_palm_link l_gripper_l_finger_link l_gripper_r_finger_link l_gripper_l_finger_tip_link l_gripper_r_finger_tip_link
+ b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+
+## list of links that the robot can see with its sensors (used to remove
+## parts of the robot from scanned data)
+self_see:
+ l_upper_arm_link
+ l_upper_arm_roll_link
+ l_elbow_flex_link
+ l_forearm_link
+ l_forearm_roll_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ l_gripper_l_finger_link
+ l_gripper_l_finger_tip_link
+ l_gripper_r_finger_link
+ l_gripper_r_finger_tip_link
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ torso_lift_link
+ base_link
+
+
+## The padding to be added for the body parts the robot can see
+self_see_padd: 0.1
+
+## The scaling to be considered for the body parts the robot can see
+self_see_scale: 1.0
Added: pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/collision/collision_checks_right_arm.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -0,0 +1,57 @@
+## This file should be loaded under <robot description>_collision
+
+
+## links for which collision checking with the environment should be performed
+collision_links:
+ base_link
+ torso_lift_link
+ head_pan_link
+ head_tilt_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_palm_link
+ r_gripper_l_finger_link
+ r_gripper_r_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_tip_link
+
+## self collision is performed between groups of links
+self_collision_groups: scg_r
+
+## -- for right arm; self-collision if any link in 'a' collides with some link in 'b'
+scg_r:
+ a: r_forearm_link r_gripper_palm_link r_gripper_l_finger_link r_gripper_r_finger_link r_gripper_l_finger_tip_link r_gripper_r_finger_tip_link
+ b: base_link torso_lift_link laser_tilt_mount_link head_tilt_link
+
+## list of links that the robot can see with its sensors (used to remove
+## parts of the robot from scanned data)
+self_see:
+ r_upper_arm_link
+ r_upper_arm_roll_link
+ r_elbow_flex_link
+ r_forearm_link
+ r_forearm_roll_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ r_gripper_l_finger_link
+ r_gripper_l_finger_tip_link
+ r_gripper_r_finger_link
+ r_gripper_r_finger_tip_link
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ torso_lift_link
+ base_link
+
+
+## The padding to be added for the body parts the robot can see
+self_see_padd: 0.1
+
+## The scaling to be considered for the body parts the robot can see
+self_see_scale: 1.0
Copied: pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml (from rev 16825, pkg/trunk/motion_planning/planning_environment/planning.yaml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/planning/planning.yaml 2009-06-07 21:59:26 UTC (rev 16826)
@@ -0,0 +1,90 @@
+## This file should be loaded under <robot description>_planning
+
+## the list of groups for which motion planning can be performed
+group_list:
+ base
+ left_arm
+ right_arm
+
+## the definition of each group
+groups:
+
+ base:
+ links:
+ base_link
+ planner_configs:
+ RRTConfig1 LazyRRTConfig1 SBLConfig1 KPIECEConfig1
+
+ left_arm:
+ links:
+ l_shoulder_pan_link
+ l_shoulder_lift_link
+ l_upper_arm_roll_link
+ l_upper_arm_link
+ l_elbow_flex_link
+ l_forearm_roll_link
+ l_forearm_link
+ l_wrist_flex_link
+ l_wrist_roll_link
+ planner_configs:
+ RRTConfig2 SBLConfig2 KPIECEConfig2l
+
+ right_arm:
+ links:
+ r_shoulder_pan_link
+ r_shoulder_lift_link
+ r_upper_arm_roll_link
+ r_upper_arm_link
+ r_elbow_flex_link
+ r_forearm_roll_link
+ r_forearm_link
+ r_wrist_flex_link
+ r_wrist_roll_link
+ planner_configs:
+ RRTConfig2 SBLConfig2 KPIECEConfig2r
+
+## the planner configurations; each config must have a type, which specifies
+## the planner to be used; other parameters can be specified as well, depending
+## on the planner
+
+planner_configs:
+
+ RRTConfig1:
+ type: RRT
+ range: 0.75
+
+ LazyRRTConfig1:
+ type: LazyRRT
+ range: 0.75
+
+ SBLConfig1:
+ type: SBL
+ projection: 0 1
+ celldim: 1 1
+ range: 0.5
+
+ KPIECEConfig1:
+ type: KPIECE
+ projection: 0 1
+ celldim: 1 1
+ range: 0.5
+
+ RRTConfig2:
+ type: RRT
+ range: 0.75
+
+ SBLConfig2:
+ type: SBL
+ projection: 5 6
+ celldim: 0.1 0.1
+
+ KPIECEConfig2l:
+ type: KPIECE
+ projection: link l_wrist_roll_link
+ celldim: 0.1 0.1 0.1
+
+ KPIECEConfig2r:
+ type: KPIECE
+ projection: link r_wrist_roll_link
+ celldim: 0.1 0.1 0.1
+
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/pr2/pr2_defs/pr2_description.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/pr2_description.launch (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/pr2_description.launch 2009-06-07 21:59:26 UTC (rev 16826)
@@ -0,0 +1,11 @@
+
+<launch>
+ <!-- send pr2.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+
+ <!-- send parameters for collision checking for PR2; this includes parameters for the self filter -->
+ <rosparam command="load" ns="robotdesc/pr2_collision" file="$(find pr2_defs)/collision/collision_checks_both_arms.yaml" />
+
+ <!-- send parameters needed for motion planning -->
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find pr2_defs)/planning/planning.yaml" />
+</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-08 05:49:13
|
Revision: 16830
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16830&view=rev
Author: isucan
Date: 2009-06-08 05:49:11 +0000 (Mon, 08 Jun 2009)
Log Message:
-----------
kinematic state + collision map monitoring in planning_environment
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/KinematicStateMonitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -81,6 +81,7 @@
while (m_nodeHandle.ok() && (m_haveMechanismState ^ loadedRobot()))
{
ROS_INFO("Waiting for mechanism state ...");
+ ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
ROS_INFO("Mechanism state received!");
@@ -88,9 +89,12 @@
void kinematic_planning::KinematicStateMonitor::waitForPose(void)
{
- ROS_INFO("Waiting for robot pose ...");
while (m_nodeHandle.ok() && (m_haveBasePos ^ loadedRobot()))
+ {
+ ROS_INFO("Waiting for robot pose ...");
+ ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
+ }
ROS_INFO("Robot pose received!");
}
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-08 05:49:11 UTC (rev 16830)
@@ -6,7 +6,9 @@
set(ROS_BUILD_TYPE Release)
rospack_add_library(planning_environment src/robot_models.cpp
- src/collision_models.cpp)
+ src/collision_models.cpp
+ src/kinematic_model_state_monitor.cpp
+ src/collision_space_monitor.cpp)
# Tests
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -39,17 +39,11 @@
#include "planning_environment/robot_models.h"
#include <collision_space/environmentODE.h>
-#include <ros/ros.h>
-#include <boost/shared_ptr.hpp>
namespace planning_environment
{
- /** @htmlinclude ../../manifest.html
-
- @mainpage
-
- A class capable of loading a robot model from the parameter server */
+ /** A class capable of loading a robot model from the parameter server */
class CollisionModels : public RobotModels
{
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,137 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#ifndef PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
+#define PLANNING_ENVIRONMENT_COLLISION_SPACE_MONITOR_
+
+#include "planning_environment/collision_models.h"
+#include "planning_environment/kinematic_model_state_monitor.h"
+
+#include <tf/transform_listener.h>
+#include <robot_msgs/CollisionMap.h>
+#include <robot_msgs/AttachedObject.h>
+
+namespace planning_environment
+{
+
+ /** @b CollisionSpaceMonitor is a class which in addition to being aware of a robot model,
+ is also aware of a collision space.
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "collision_map"/CollisionMap : data describing the 3D environment
+ - @b "attach_object"/AttachedObject : data describing an object to be attached to a link
+ */
+ class CollisionSpaceMonitor : public KinematicModelStateMonitor
+ {
+ public:
+
+ CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), true),
+ tf_(*ros::Node::instance())
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
+ virtual ~CollisionSpaceMonitor(void)
+ {
+ }
+
+ collision_space::EnvironmentModel* getEnvironmentModel(void) const
+ {
+ return collisionSpace_;
+ }
+
+ CollisionModels* getCollisionModels(void) const
+ {
+ return cm_;
+ }
+
+ /** Define a callback for before updating a map */
+ void setOnBeforeMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ {
+ onBeforeMapUpdate_ = callback;
+ }
+
+ /** Define a callback for after updating a map */
+ void setOnAfterMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
+ {
+ onAfterMapUpdate_ = callback;
+ }
+
+ /** Define a callback for after updating a map */
+ void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
+ {
+ onAfterAttachBody_ = callback;
+ }
+
+ bool haveMap(void) const
+ {
+ return haveMap_;
+ }
+
+ /** Return true if a map update has been received in the last sec seconds */
+ bool isMapUpdated(double sec) const;
+
+ protected:
+
+ void setupCSM(void);
+ void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
+ void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
+
+ tf::TransformListener tf_;
+ CollisionModels *cm_;
+ collision_space::EnvironmentModel *collisionSpace_;
+
+ bool haveMap_;
+ ros::Time lastMapUpdate_;
+ ros::Subscriber attachBodySubscriber_;
+ ros::Subscriber collisionMapSubscriber_;
+
+ boost::function<void(const robot_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
+ boost::function<void(const robot_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
+ boost::function<void(planning_models::KinematicModel::Link*)> onAfterAttachBody_;
+
+ };
+
+
+}
+
+#endif
+
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,159 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#ifndef PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
+#define PLANNING_ENVIRONMENT_KINEMATIC_MODEL_STATE_MONITOR_
+
+#include "planning_environment/robot_models.h"
+#include <tf/transform_datatypes.h>
+#include <robot_msgs/MechanismState.h>
+#include <robot_msgs/PoseWithCovariance.h>
+#include <boost/bind.hpp>
+#include <vector>
+#include <string>
+
+namespace planning_environment
+{
+
+ /** @b KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in @b RobotModels
+
+ <hr>
+
+ @section topic ROS topics
+
+ Subscribes to (name/type):
+ - @b "mechanism_model"/MechanismModel : position for each of the robot's joints
+ - @b "localized_pose"/PoseWithCovariance : localized robot pose
+
+ */
+ class KinematicModelStateMonitor
+ {
+ public:
+
+ KinematicModelStateMonitor(RobotModels *rm, bool includePose = false)
+ {
+ rm_ = rm;
+ includePose_ = includePose;
+ setupRSM();
+ }
+
+ virtual ~KinematicModelStateMonitor(void)
+ {
+ if (robotState_)
+ delete robotState_;
+ }
+
+ /** Define a callback for when the state is changed */
+ void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
+ {
+ onStateUpdate_ = callback;
+ }
+
+ planning_models::KinematicModel* getKinematicModel(void) const
+ {
+ return kmodel_;
+ }
+
+ RobotModels* getRobotModels(void) const
+ {
+ return rm_;
+ }
+
+ /** Return a pointer to the maintained robot state */
+ const planning_models::KinematicModel::StateParams* getRobotState(void) const
+ {
+ return robotState_;
+ }
+
+ /** Return true if a pose has been received */
+ bool havePose(void) const
+ {
+ return havePose_;
+ }
+
+ /** Return true if a full mechanism state has been received */
+ bool haveState(void) const
+ {
+ return haveMechanismState_;
+ }
+
+ /** Wait until a pose is received */
+ void waitForPose(void) const;
+
+ /** Wait until a full mechanism state is received */
+ void waitForState(void) const;
+
+ /** Return true if a pose has been received in the last sec seconds */
+ bool isPoseUpdated(double sec) const;
+
+ /** Return true if a full mechanism state has been received in the last sec seconds */
+ bool isStateUpdated(double sec) const;
+
+ /** Output the current state as ROS INFO */
+ void printRobotState(void) const;
+
+ protected:
+
+ void setupRSM(void);
+ void localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose);
+ void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
+
+
+ RobotModels *rm_;
+ bool includePose_;
+ planning_models::KinematicModel *kmodel_;
+ std::vector<std::string> planarJoints_;
+ std::vector<std::string> floatingJoints_;
+
+ ros::NodeHandle nh_;
+ ros::Subscriber mechanismStateSubscriber_;
+ ros::Subscriber localizedPoseSubscriber_;
+
+ planning_models::KinematicModel::StateParams *robotState_;
+ tf::Pose pose_;
+ boost::function<void(void)> onStateUpdate_;
+
+ bool havePose_;
+ bool haveMechanismState_;
+ ros::Time lastStateUpdate_;
+ ros::Time lastPoseUpdate_;
+ };
+
+
+}
+
+#endif
+
Property changes on: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-08 05:49:11 UTC (rev 16830)
@@ -37,7 +37,7 @@
#ifndef PLANNING_ENVIRONMENT_ROBOT_MODELS_
#define PLANNING_ENVIRONMENT_ROBOT_MODELS_
-#include "planning_models/kinematic.h"
+#include <planning_models/kinematic.h>
#include <ros/ros.h>
#include <ros/node.h>
#include <boost/shared_ptr.hpp>
@@ -49,11 +49,7 @@
namespace planning_environment
{
- /** @htmlinclude ../../manifest.html
-
- @mainpage
-
- A class capable of loading a robot model from the parameter server */
+ /** A class capable of loading a robot model from the parameter server */
class RobotModels
{
@@ -85,6 +81,7 @@
RobotModels(const std::string &description)
{
description_ = nh_.getNode()->mapName(description);
+ loaded_models_ = false;
loadRobot();
}
@@ -129,22 +126,18 @@
return self_collision_check_groups_;
}
+ bool loadedModels(void) const
+ {
+ return loaded_models_;
+ }
+
double getSelfSeePadding(void);
double getSelfSeeScale(void);
std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
/** Reload the robot description and recreate the model */
- virtual void reload(void)
- {
- kmodel_.reset();
- urdf_.reset();
- planning_groups_.clear();
- self_see_links_.clear();
- collision_check_links_.clear();
- self_collision_check_groups_.clear();
- loadRobot();
- }
+ virtual void reload(void);
protected:
@@ -160,6 +153,7 @@
std::string description_;
+ bool loaded_models_;
boost::shared_ptr<planning_models::KinematicModel> kmodel_;
boost::shared_ptr<robot_desc::URDF> urdf_;
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-08 05:49:11 UTC (rev 16830)
@@ -11,7 +11,9 @@
<depend package="roscpp" />
<depend package="rosconsole" />
+ <depend package="tf" />
<depend package="pr2_defs" />
+ <depend package="robot_msgs" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-08 05:08:22 UTC (rev 16829)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_models.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -35,14 +35,12 @@
/** \author Ioan Sucan */
#include "planning_environment/collision_models.h"
-#include <ros/console.h>
-#include <sstream>
void planning_environment::CollisionModels::loadCollision(void)
{
- ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
- if (urdf_.get() && kmodel_.get())
+ if (loadedModels())
{
+ ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
ode_collision_model_->lock();
ode_collision_model_->addRobotModel(kmodel_, collision_check_links_, scale_, padd_);
Added: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,149 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include "planning_environment/collision_space_monitor.h"
+
+namespace planning_environment
+{
+
+ static inline double radiusOfBox(const robot_msgs::Point32 &point)
+ {
+ return std::max(std::max(point.x, point.y), point.z) * 1.73;
+ }
+}
+
+void planning_environment::CollisionSpaceMonitor::setupCSM(void)
+{
+ onBeforeMapUpdate_ = NULL;
+ onAfterMapUpdate_ = NULL;
+ onAfterAttachBody_ = NULL;
+ haveMap_ = false;
+ collisionSpace_ = cm_->getODECollisionModel().get();
+
+ collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
+ attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+}
+
+bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
+{
+ if (sec > 0 && lastMapUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
+void planning_environment::CollisionSpaceMonitor::collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap)
+{
+ unsigned int n = collisionMap->get_boxes_size();
+ ROS_DEBUG("Received %u points (collision map)", n);
+
+ if (onBeforeMapUpdate_ != NULL)
+ onBeforeMapUpdate_(collisionMap);
+
+ ros::WallTime startTime = ros::WallTime::now();
+ double *data = new double[4 * n];
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ unsigned int i4 = i * 4;
+ data[i4 ] = collisionMap->boxes[i].center.x;
+ data[i4 + 1] = collisionMap->boxes[i].center.y;
+ data[i4 + 2] = collisionMap->boxes[i].center.z;
+
+ data[i4 + 3] = radiusOfBox(collisionMap->boxes[i].extents);
+ }
+
+ collisionSpace_->lock();
+ collisionSpace_->clearObstacles();
+ collisionSpace_->addPointCloud(n, data);
+ collisionSpace_->unlock();
+
+ delete[] data;
+
+ double tupd = (ros::WallTime::now() - startTime).toSec();
+ ROS_DEBUG("Updated map model in %f seconds", tupd);
+ lastMapUpdate_ = ros::Time::now();
+ haveMap_ = true;
+
+ if (onAfterMapUpdate_ != NULL)
+ onAfterMapUpdate_(collisionMap);
+}
+
+void planning_environment::CollisionSpaceMonitor::attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject)
+{
+ collisionSpace_->lock();
+ planning_models::KinematicModel::Link *link = kmodel_->getLink(attachedObject->link_name);
+
+ if (link)
+ {
+ // clear the previously attached bodies
+ for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
+ delete link->attachedBodies[i];
+ unsigned int n = attachedObject->get_objects_size();
+ link->attachedBodies.resize(n);
+
+ // create the new ones
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ link->attachedBodies[i] = new planning_models::KinematicModel::AttachedBody();
+
+ robot_msgs::PointStamped center;
+ robot_msgs::PointStamped centerP;
+ center.point.x = attachedObject->objects[i].center.x;
+ center.point.y = attachedObject->objects[i].center.y;
+ center.point.z = attachedObject->objects[i].center.z;
+ center.header = attachedObject->header;
+ tf_.transformPoint(attachedObject->link_name, center, centerP);
+
+ link->attachedBodies[i]->attachTrans.setOrigin(btVector3(centerP.point.x, centerP.point.y, centerP.point.z));
+
+ // this is a HACK! we should have orientation
+ planning_models::shapes::Box *box = new planning_models::shapes::Box();
+ box->size[0] = attachedObject->objects[i].max_bound.x - attachedObject->objects[i].min_bound.x;
+ box->size[1] = attachedObject->objects[i].max_bound.y - attachedObject->objects[i].min_bound.y;
+ box->size[2] = attachedObject->objects[i].max_bound.z - attachedObject->objects[i].min_bound.z;
+ link->attachedBodies[i]->shape = box;
+ }
+
+ // update the collision model
+ collisionSpace_->updateAttachedBodies();
+ ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
+ }
+ else
+ ROS_WARN("Unable to attach object to link '%s' on '%s'", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str());
+ collisionSpace_->unlock();
+ if (link && (onAfterAttachBody_ != NULL))
+ onAfterAttachBody_(link);
+}
Added: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-08 05:49:11 UTC (rev 16830)
@@ -0,0 +1,196 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include "planning_environment/kinematic_model_state_monitor.h"
+#include <sstream>
+
+void planning_environment::KinematicModelStateMonitor::setupRSM(void)
+{
+ pose_.setIdentity();
+ kmodel_ = NULL;
+ robotState_ = NULL;
+ onStateUpdate_ = NULL;
+ havePose_ = haveMechanismState_ = false;
+ if (rm_->loadedModels())
+ {
+ kmodel_ = rm_->getKinematicModel().get();
+ robotState_ = kmodel_->newStateParams();
+ robotState_->setInRobotFrame();
+
+ for (unsigned int i = 0 ; i < kmodel_->getModelInfo().floatingJoints.size() ; ++i)
+ floatingJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().floatingJoints[i]]);
+ for (unsigned int i = 0 ; i < kmodel_->getModelInfo().planarJoints.size() ; ++i)
+ planarJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().planarJoints[i]]);
+
+ mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
+ if (includePose_)
+ localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
+ }
+}
+
+void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
+{
+ bool change = !haveMechanismState_;
+
+ static bool first_time = true;
+
+ unsigned int n = mechanismState->get_joint_states_size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ planning_models::KinematicModel::Joint* joint = kmodel_->getJoint(mechanismState->joint_states[i].name);
+ if (joint)
+ {
+ if (joint->usedParams == 1)
+ {
+ double pos = mechanismState->joint_states[i].position;
+ bool this_changed = robotState_->setParamsJoint(&pos, mechanismState->joint_states[i].name);
+ change = change || this_changed;
+ }
+ else
+ {
+ if (first_time)
+ ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", mechanismState->joint_states[i].name.c_str(), joint->usedParams);
+ }
+ }
+ else
+ {
+ if (first_time)
+ ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
+ }
+ }
+
+ first_time = false;
+
+ lastStateUpdate_ = ros::Time::now();
+ if (!haveMechani...
[truncated message content] |
|
From: <tf...@us...> - 2009-06-08 22:46:50
|
Revision: 16840
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16840&view=rev
Author: tfoote
Date: 2009-06-08 22:46:38 +0000 (Mon, 08 Jun 2009)
Log Message:
-----------
creating audio_msgs packge ticket:1321
Modified Paths:
--------------
pkg/trunk/audio/audio_capture/audio_capture.cpp
pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
pkg/trunk/audio/audio_capture/manifest.xml
Added Paths:
-----------
pkg/trunk/audio/audio_msgs/
pkg/trunk/audio/audio_msgs/CMakeLists.txt
pkg/trunk/audio/audio_msgs/Makefile
pkg/trunk/audio/audio_msgs/mainpage.dox
pkg/trunk/audio/audio_msgs/manifest.xml
pkg/trunk/audio/audio_msgs/msg/
pkg/trunk/audio/audio_msgs/msg/AudioRawStream.msg
Removed Paths:
-------------
pkg/trunk/common/robot_msgs/msg/AudioRawStream.msg
Modified: pkg/trunk/audio/audio_capture/audio_capture.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-06-08 21:33:32 UTC (rev 16839)
+++ pkg/trunk/audio/audio_capture/audio_capture.cpp 2009-06-08 22:46:38 UTC (rev 16840)
@@ -30,7 +30,7 @@
#include <cstdio>
#include <portaudio.h>
#include "ros/node.h"
-#include "robot_msgs/AudioRawStream.h"
+#include "audio_msgs/AudioRawStream.h"
const static int SAMPLE_RATE = 44100; // todo: make this a parameter.
ros::Node *g_audio_node = NULL;
@@ -44,7 +44,7 @@
const PaStreamCallbackTimeInfo *time_info,
PaStreamCallbackFlags status, void *user_data)
{
- static robot_msgs::AudioRawStream audio_msg;
+ static audio_msgs::AudioRawStream audio_msg;
audio_msg.num_channels = 1;
audio_msg.sample_rate = SAMPLE_RATE;
#ifdef GRATUITOUS_DEBUGGING
@@ -94,7 +94,7 @@
ros::init(argc, argv);
ros::Node n("audio_capture", ros::Node::DONT_HANDLE_SIGINT);
g_audio_node = &n;
- n.advertise<robot_msgs::AudioRawStream>("audio", 5);
+ n.advertise<audio_msgs::AudioRawStream>("audio", 5);
err = Pa_OpenDefaultStream(&stream, 1, 0, paFloat32, SAMPLE_RATE, 4096,
audio_cb, NULL);
if (err != paNoError)
Modified: pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2009-06-08 21:33:32 UTC (rev 16839)
+++ pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2009-06-08 22:46:38 UTC (rev 16840)
@@ -31,7 +31,7 @@
#include <deque>
#include <vector>
#include "ros/node.h"
-#include "robot_msgs/AudioRawStream.h"
+#include "audio_msgs/AudioRawStream.h"
#include <sndfile.h>
using std::deque;
@@ -156,7 +156,7 @@
uint64_t audio_clock;
enum { IDLE, CLIP_START, IN_CLIP } clip_state;
float window_power;
- robot_msgs::AudioRawStream audio_msg;
+ audio_msgs::AudioRawStream audio_msg;
deque<float> window;
vector<float> clip;
};
Modified: pkg/trunk/audio/audio_capture/manifest.xml
===================================================================
--- pkg/trunk/audio/audio_capture/manifest.xml 2009-06-08 21:33:32 UTC (rev 16839)
+++ pkg/trunk/audio/audio_capture/manifest.xml 2009-06-08 22:46:38 UTC (rev 16840)
@@ -4,7 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="robot_msgs"/>
+ <depend package="audio_msgs"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="portaudio19-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="portaudio19-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libsndfile1-dev"/>
Added: pkg/trunk/audio/audio_msgs/CMakeLists.txt
===================================================================
--- pkg/trunk/audio/audio_msgs/CMakeLists.txt (rev 0)
+++ pkg/trunk/audio/audio_msgs/CMakeLists.txt 2009-06-08 22:46:38 UTC (rev 16840)
@@ -0,0 +1,30 @@
+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(audio_msgs)
+
+#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(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
Added: pkg/trunk/audio/audio_msgs/Makefile
===================================================================
--- pkg/trunk/audio/audio_msgs/Makefile (rev 0)
+++ pkg/trunk/audio/audio_msgs/Makefile 2009-06-08 22:46:38 UTC (rev 16840)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Added: pkg/trunk/audio/audio_msgs/mainpage.dox
===================================================================
--- pkg/trunk/audio/audio_msgs/mainpage.dox (rev 0)
+++ pkg/trunk/audio/audio_msgs/mainpage.dox 2009-06-08 22:46:38 UTC (rev 16840)
@@ -0,0 +1,107 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b audio_msgs is ... In addition to providing an overview of your package,
+this is the section where the specification and design/architecture
+should be detailed. While the original specification may be done on the
+wiki, it should be transferred here once your package starts to take shape.
+You can then link to this documentation page from the Wiki.
+
+
+\section codeapi Code API
+
+Provide links to specific auto-generated API documentation within your
+package that is of particular interest to a reader. Doxygen will
+document pretty much every part of your code, so do your best here to
+point the reader to the actual API.
+
+If your codebase is fairly large or has different sets of APIs, you
+should use the doxygen 'group' tag to keep these APIs together. For
+example, the roscpp documentation has 'libros' group
+so that it can be viewed separately. The rospy documentation
+similarly has a 'client-api' group that pulls together APIs for a
+Client API page.
+
+
+\section rosapi ROS API
+
+Names are very important in ROS because they can be remapped on the
+command-line, so it is VERY IMPORTANT THAT YOU LIST NAMES AS THEY
+APPEAR IN THE CODE. You should list names of every topic, service and
+parameter used in your code.
+
+List of nodes:
+- \b node_name1
+- \b node_name2
+
+<!-- START: copy for each node -->
+
+<hr>
+
+\subsection node_name node_name
+
+node_name does (provide a basic description of your node)
+
+\subsubsection Usage
+\verbatim
+$ node_type1 [standard ROS args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ node_type1
+\endverbatim
+
+
+\subsubsection topics ROS topics
+
+Subscribes to:
+- \b "in": [std_msgs/FooType] description of in
+
+Publishes to:
+- \b "out": [std_msgs/FooType] description of out
+
+\subsubsection parameters ROS parameters
+
+Reads the following parameters from the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+- \b "~my_param" : \b [string] description of my_param
+
+Sets the following parameters on the parameter server
+
+- \b "~param_name" : \b [type] description of param_name
+
+
+\subsubsection services ROS services
+- \b "foo_service": [std_srvs/FooType] description of foo_service
+
+
+<!-- END: copy for each node -->
+
+\section commandline Command-line tools
+
+This section is a catch-all for any additional tools that your package
+provides or uses that may be of use to the reader. For example:
+
+- tools/scripts (e.g. rospack, roscd)
+- roslaunch .launch files
+- xmlparam files
+
+\subsection script_name script_name
+
+Description of what this script/file does.
+
+\subsubsection Usage
+\verbatim
+$ ./script_name [args]
+\endverbatim
+
+\par Example
+
+\verbatim
+$ ./script_name foo bar
+\endverbatim
+*/
\ No newline at end of file
Added: pkg/trunk/audio/audio_msgs/manifest.xml
===================================================================
--- pkg/trunk/audio/audio_msgs/manifest.xml (rev 0)
+++ pkg/trunk/audio/audio_msgs/manifest.xml 2009-06-08 22:46:38 UTC (rev 16840)
@@ -0,0 +1,18 @@
+<package>
+ <description brief="audio_msgs">
+
+ audio_msgs
+
+ </description>
+ <author>Morgan Quigley<author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/audio_msgs</url>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+
+
+</package>
+
+
Copied: pkg/trunk/audio/audio_msgs/msg/AudioRawStream.msg (from rev 14615, pkg/trunk/common/robot_msgs/msg/AudioRawStream.msg)
===================================================================
--- pkg/trunk/audio/audio_msgs/msg/AudioRawStream.msg (rev 0)
+++ pkg/trunk/audio/audio_msgs/msg/AudioRawStream.msg 2009-06-08 22:46:38 UTC (rev 16840)
@@ -0,0 +1,4 @@
+float32[] samples
+uint32 num_channels
+uint32 sample_rate
+
Deleted: pkg/trunk/common/robot_msgs/msg/AudioRawStream.msg
===================================================================
--- pkg/trunk/common/robot_msgs/msg/AudioRawStream.msg 2009-06-08 21:33:32 UTC (rev 16839)
+++ pkg/trunk/common/robot_msgs/msg/AudioRawStream.msg 2009-06-08 22:46:38 UTC (rev 16840)
@@ -1,4 +0,0 @@
-float32[] samples
-uint32 num_channels
-uint32 sample_rate
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-08 23:58:00
|
Revision: 16843
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16843&view=rev
Author: tfoote
Date: 2009-06-08 23:57:45 +0000 (Mon, 08 Jun 2009)
Log Message:
-----------
moving transform broadcaster to NodeHandle API
Modified Paths:
--------------
pkg/trunk/common/tf/include/tf/transform_broadcaster.h
pkg/trunk/common/tf/src/transform_broadcaster.cpp
pkg/trunk/common/tf/src/transform_sender.cpp
pkg/trunk/common/tf/test/testBroadcaster.cpp
pkg/trunk/common/tf/test/test_notifier.cpp
pkg/trunk/common/tf/tutorials/earth_tracker.cpp
pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp
pkg/trunk/deprecated/amcl_player/amcl_player.cc
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
pkg/trunk/simulators/stage/src/stageros.cpp
pkg/trunk/vision/outlet_detection/src/tracker_base.cpp
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/visualization/rviz/src/test/cloud_test.cpp
pkg/trunk/visualization/rviz/src/test/marker_test.cpp
Modified: pkg/trunk/common/tf/include/tf/transform_broadcaster.h
===================================================================
--- pkg/trunk/common/tf/include/tf/transform_broadcaster.h 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/include/tf/transform_broadcaster.h 2009-06-08 23:57:45 UTC (rev 16843)
@@ -33,10 +33,17 @@
#ifndef TF_TRANSFORMBROADCASTER_H
#define TF_TRANSFORMBROADCASTER_H
-#include "ros/node.h"
#include "tf/tf.h"
#include "tf/tfMessage.h"
+//Forward declaring not working
+#include "ros/ros.h"
+/*namespace ros
+{
+class NodeHandle;
+class Publisher;
+}
+*/
namespace tf
{
@@ -48,7 +55,7 @@
class TransformBroadcaster{
public:
/** \brief Constructor (needs a ros::Node reference) */
- TransformBroadcaster(ros::Node& anode);
+ TransformBroadcaster();
/** \brief Send a Stamped<Transform> with parent parent_id
* The stamped data structure includes frame_id, and time, and parent_id already. */
@@ -59,7 +66,8 @@
private:
/// Internal reference to ros::Node
- ros::Node & node_;
+ ros::NodeHandle node_;
+ ros::Publisher publisher_;
std::string tf_prefix_;
Modified: pkg/trunk/common/tf/src/transform_broadcaster.cpp
===================================================================
--- pkg/trunk/common/tf/src/transform_broadcaster.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/src/transform_broadcaster.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -31,14 +31,14 @@
/** \author Tully Foote */
+#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
namespace tf {
-TransformBroadcaster::TransformBroadcaster(ros::Node& anode):
- node_(anode)
+TransformBroadcaster::TransformBroadcaster()
{
- node_.advertise<tfMessage>("/tf_message", 100);
+ publisher_ = node_.advertise<tfMessage>("/tf_message", 100);
node_.param(std::string("~tf_prefix"),tf_prefix_, std::string(""));
};
@@ -50,7 +50,7 @@
msgtf.header.frame_id = tf::remap(tf_prefix_, msgtf.header.frame_id);
msgtf.parent_id = tf::remap(tf_prefix_, msgtf.parent_id);
message.transforms.push_back(msgtf);
- node_.publish("/tf_message", message);
+ publisher_.publish(message);
}
@@ -64,7 +64,7 @@
msgtf.parent_id = remap(tf_prefix_, parent_id);
TransformTFToMsg(transform, msgtf.transform);
message.transforms.push_back(msgtf);
- node_.publish("/tf_message", message);
+ publisher_.publish(message);
}
Modified: pkg/trunk/common/tf/src/transform_sender.cpp
===================================================================
--- pkg/trunk/common/tf/src/transform_sender.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/src/transform_sender.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -32,10 +32,9 @@
class TransformSender
{
public:
- ros::Node node_;
+ ros::NodeHandle node_;
//constructor
TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& parent_id) :
- node_("transform_sender", ros::Node::ANONYMOUS_NAME),broadcaster(node_),
transform_(btTransform(btQuaternion(yaw,pitch,roll), btVector3(x,y,z)), time, frame_id , parent_id){};
//Clean up ros connections
~TransformSender() { }
@@ -59,7 +58,7 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
+ ros::init(argc, argv,"transform_sender", ros::init_options::AnonymousName);
if(argc != 10)
{
Modified: pkg/trunk/common/tf/test/testBroadcaster.cpp
===================================================================
--- pkg/trunk/common/tf/test/testBroadcaster.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/test/testBroadcaster.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -28,12 +28,13 @@
*/
#include "tf/transform_broadcaster.h"
+#include "ros/ros.h"
-class testBroadcaster : public ros::Node
+class testBroadcaster
{
public:
//constructor
- testBroadcaster() : ros::Node("broadcaster"),broadcaster(*this),count(2){};
+ testBroadcaster() : count(2){};
//Clean up ros connections
~testBroadcaster() { }
@@ -68,7 +69,8 @@
//Construct/initialize the server
testBroadcaster myTestBroadcaster;
- while(myTestBroadcaster.ok())
+ ros::NodeHandle node; //\todo replace with ros::ok() after 0.5.2 release
+ while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
//Send some data
myTestBroadcaster.test();
Modified: pkg/trunk/common/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/common/tf/test/test_notifier.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/test/test_notifier.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -567,7 +567,7 @@
g_node->advertise<robot_msgs::PointStamped>("test_message2", 0);
g_tf = new TransformListener(*g_node);
- g_broadcaster = new TransformBroadcaster(*g_node);
+ g_broadcaster = new TransformBroadcaster();
int ret = RUN_ALL_TESTS();
Modified: pkg/trunk/common/tf/tutorials/earth_tracker.cpp
===================================================================
--- pkg/trunk/common/tf/tutorials/earth_tracker.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/tutorials/earth_tracker.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -30,13 +30,14 @@
/**\author Tully Foote/tfoote at willowgarage.com */
#include "tf/transform_broadcaster.h"
+#include "ros/ros.h"
#include <cmath>
class EarthTracker
{
public:
//constructor
- EarthTracker(ros::Node& node) : broadcaster(node),count(2){};
+ EarthTracker(): count(2){};
//Clean up ros connections
~EarthTracker() { }
@@ -64,12 +65,12 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
- ros::Node node("earth_tracker");
+ ros::init(argc, argv, "earth_tracker");
//Construct/initialize the server
- EarthTracker myEarthTracker(node);
+ EarthTracker myEarthTracker;
+ ros::NodeHandle node; //\todo replace with ros::ok() after 0.5.2 release
while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
//Send the position of the earth with respect to the sun
Modified: pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp
===================================================================
--- pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/common/tf/tutorials/space_shuttle_tracker.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -36,10 +36,10 @@
{
public:
//constructor
- SpaceShuttleTracker(ros::Node& node) : broadcaster(node),count(2){};
+ SpaceShuttleTracker() : count(2){}
//Clean up ros connections
~SpaceShuttleTracker() { }
-
+
//A pointer to the rosTFServer class
tf::TransformBroadcaster broadcaster;
@@ -83,11 +83,11 @@
int main(int argc, char ** argv)
{
//Initialize ROS
- ros::init(argc, argv);
- ros::Node node("nasa_tracking");
+ ros::init(argc, argv,"nasa_tracking");
+ ros::NodeHandle node;//\todo replace with ros::ok() when 0.5.2 is released
//Construct/initialize the server
- SpaceShuttleTracker mySpaceShuttleTracker(node);
+ SpaceShuttleTracker mySpaceShuttleTracker;
while(node.ok())//Check if a Ctrl-C or other shutdown command has been recieved
{
Modified: pkg/trunk/deprecated/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/deprecated/amcl_player/amcl_player.cc 2009-06-08 23:57:45 UTC (rev 16843)
@@ -436,7 +436,7 @@
this->setPose(startX, startY, startTH);
cloud_pub_interval.fromSec(1.0);
- this->tf = new tf::TransformBroadcaster(*this);
+ this->tf = new tf::TransformBroadcaster();
this->tfL = new tf::TransformListener(*this);
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",2);
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-06-08 23:57:45 UTC (rev 16843)
@@ -108,7 +108,7 @@
tf::TransformBroadcaster tf;
ErraticNode() : ros::Node("erratic_player"),
- tf(*this), watts_charging_(10), watts_unplugged_(-10), charging_threshold_(12.98)
+ watts_charging_(10), watts_unplugged_(-10), charging_threshold_(12.98)
{
// libplayercore boiler plate
player_globals_init();
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -77,7 +77,7 @@
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose", 1);
advertise<robot_msgs::PoseWithRatesStamped>("base_pose_ground_truth", 1) ;
- m_tfServer = new tf::TransformBroadcaster(*this);
+ m_tfServer = new tf::TransformBroadcaster();
subscribe("phase_space_snapshot", snapshot_, &PhaseSpaceLocalization::snapshotCallback, 10) ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -91,7 +91,7 @@
ros::Node::instance()->advertise<deprecated_msgs::RobotBase2DOdom>("odom", 1);
deprecated_msgs::RobotBase2DOdom odom;
- tf::TransformBroadcaster tfb(*ros::Node::instance());
+ tf::TransformBroadcaster tfb;
ros::Duration d; d.fromSec(0.01);
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -56,7 +56,6 @@
: ros::Node(node_name),
node_name_(node_name),
robot_state_(*this, true),
- odom_broadcaster_(*this),
vo_notifier_(NULL),
vel_desi_(2),
vel_active_(false),
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -66,7 +66,7 @@
string file_name_, cloud_topic_;
double rate_;
- PCDGenerator (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode), broadcaster_ (anode),
+ PCDGenerator (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode),
transform_ (btTransform (btQuaternion (0, 0, 0), btVector3 (0, 0, 0)), ros::Time::now (), tf_frame_, tf_frame_)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -396,7 +396,7 @@
(M_PI/12.0) * (M_PI/12.0));
cloud_pub_interval.fromSec(1.0);
- tfb_ = new tf::TransformBroadcaster(*ros::Node::instance());
+ tfb_ = new tf::TransformBroadcaster();
tf_ = new tf::TransformListener(*ros::Node::instance());
map_ = requestMap();
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -93,7 +93,7 @@
{
advertise<robot_msgs::PoseWithCovariance>("amcl_pose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
- m_tfServer = new tf::TransformBroadcaster(*this);
+ m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener(*this);
m_lastUpdate = ros::Time::now();
Modified: pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp
===================================================================
--- pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/nav/visual_nav/src/ros_visual_nav.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -279,7 +279,7 @@
// Constructor
RosVisualNavigator::RosVisualNavigator (double exit_point_radius, uint scan_period) :
- node_("visual_navigator"), tf_listener_(node_), tf_sender_(node_), map_received_(false),
+ node_("visual_navigator"), tf_listener_(node_), map_received_(false),
odom_received_(false), exit_point_radius_(exit_point_radius), have_goal_(false),
num_active_markers_(0), scan_counter_(1), scan_period_(scan_period)
{
Modified: pkg/trunk/simulators/stage/src/stageros.cpp
===================================================================
--- pkg/trunk/simulators/stage/src/stageros.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/simulators/stage/src/stageros.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -199,8 +199,7 @@
}
StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) :
- ros::Node("stageros"),
- tf(*this)
+ ros::Node("stageros")
{
this->sim_time.fromSec(0.0);
this->base_last_cmd.fromSec(0.0);
Modified: pkg/trunk/vision/outlet_detection/src/tracker_base.cpp
===================================================================
--- pkg/trunk/vision/outlet_detection/src/tracker_base.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/vision/outlet_detection/src/tracker_base.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -15,7 +15,7 @@
TrackerBase::TrackerBase(ros::Node &node, std::string prefix)
: node_(node), img_(res_.image), cam_info_(res_.cam_info),
pose_topic_name_("~" + prefix + "_pose"),
- tf_broadcaster_(node), tf_listener_(node),
+ tf_listener_(node),
object_frame_id_(prefix + "_pose"), K_(NULL),
display_topic_name_("~" + prefix + "_image"),
save_count_(0), save_prefix_(prefix)
Modified: pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -95,7 +95,7 @@
std_msgs::String msg_prefix;
- PublishScene (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode), broadcaster_ (anode),
+ PublishScene (ros::Node& anode) : tf_frame_ ("base_link"), node_ (anode),
transform_ (btTransform (btQuaternion (0, 0, 0), btVector3 (0, 0, 0)), ros::Time::now (), tf_frame_, tf_frame_)
{
// Maximum number of outgoing messages to be queued for delivery to subscribers = 1
Modified: pkg/trunk/visualization/rviz/src/test/cloud_test.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/test/cloud_test.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/visualization/rviz/src/test/cloud_test.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -20,7 +20,7 @@
node->advertise<robot_msgs::PointCloud>( "intensity_cloud_test", 0 );
node->advertise<robot_msgs::PointCloud>( "million_points_cloud_test", 0 );
- tf::TransformBroadcaster tf_broadcaster(*node);
+ tf::TransformBroadcaster tf_broadcaster;
usleep( 1000000 );
Modified: pkg/trunk/visualization/rviz/src/test/marker_test.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-06-08 23:05:20 UTC (rev 16842)
+++ pkg/trunk/visualization/rviz/src/test/marker_test.cpp 2009-06-08 23:57:45 UTC (rev 16843)
@@ -20,7 +20,7 @@
node->advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
node->advertise<visualization_msgs::MarkerArray>( "visualization_marker_array", 0 );
- tf::TransformBroadcaster tf_broadcaster(*node);
+ tf::TransformBroadcaster tf_broadcaster;
usleep( 1000000 );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-06-09 01:15:03
|
Revision: 16847
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16847&view=rev
Author: eitanme
Date: 2009-06-09 01:14:54 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
Beginning to clean up the navstack.
-TrajectoryPlannerROS no longer subscribes to base/tilt independently of its costmap
-Support for specifying the robot footprint on the parameter server
-Cleaned up the WorldModel interface, this removes support for the freespace controller until it can be updated accordingly (needs a rolling window version)
-Cleaned up the TrajectoryPlanner code a bit, still needs some more work, but getting there
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml
pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -94,8 +94,6 @@
*/
void makePlan(const robot_msgs::PoseStamped& goal);
- bool escape(double escape_dist, unsigned int max_attempts, const robot_msgs::PoseStamped& robot_pose);
-
/**
* @brief Publish a goal to the visualizer
* @param goal The goal to visualize
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -44,6 +44,11 @@
using namespace robot_actions;
namespace move_base {
+
+ double sign(double x){
+ return x < 0.0 ? -1.0 : 1.0;
+ }
+
MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
@@ -69,25 +74,54 @@
robot_msgs::Point pt;
double padding;
ros_node_.param("~footprint_padding", padding, 0.01);
- //create a square footprint
- pt.x = inscribed_radius_ + padding;
- pt.y = -1 * (inscribed_radius_ + padding);
- footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + padding);
- pt.y = -1 * (inscribed_radius_ + padding);
- footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + padding);
- pt.y = inscribed_radius_ + padding;
- footprint_.push_back(pt);
- pt.x = inscribed_radius_ + padding;
- pt.y = inscribed_radius_ + padding;
- footprint_.push_back(pt);
- //give the robot a nose
- pt.x = circumscribed_radius_;
- pt.y = 0;
- footprint_.push_back(pt);
+ //grab the footprint from the parameter server if possible
+ XmlRpc::XmlRpcValue footprint_list;
+ if(ros_node_.getParam("~footprint", footprint_list)){
+ //make sure we have a list of lists
+ ROS_ASSERT_MSG(footprint_list.getType() == XmlRpcValue::TypeArray && footprint_list.size() > 2,
+ "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+ for(int i = 0; i < footprint_list.size(); ++i){
+ //make sure we have a list of lists of size 2
+ XmlRpc::XmlRpcValue point = footprint_list[i];
+ ROS_ASSERT_MSG(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2,
+ "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
+ //make sure that the value we're looking at is either a double or an int
+ ROS_ASSERT(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+ pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
+ pt.x += sign(pt.x) * padding;
+
+ //make sure that the value we're looking at is either a double or an int
+ ROS_ASSERT(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
+ pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
+ pt.y += sign(pt.y) * padding;
+
+ footprint_.push_back(pt);
+
+ }
+ }
+ else{
+ //if no explicit footprint is set on the param server... create a square footprint
+ pt.x = inscribed_radius_ + padding;
+ pt.y = -1 * (inscribed_radius_ + padding);
+ footprint_.push_back(pt);
+ pt.x = -1 * (inscribed_radius_ + padding);
+ pt.y = -1 * (inscribed_radius_ + padding);
+ footprint_.push_back(pt);
+ pt.x = -1 * (inscribed_radius_ + padding);
+ pt.y = inscribed_radius_ + padding;
+ footprint_.push_back(pt);
+ pt.x = inscribed_radius_ + padding;
+ pt.y = inscribed_radius_ + padding;
+ footprint_.push_back(pt);
+
+ //give the robot a nose
+ pt.x = circumscribed_radius_;
+ pt.y = 0;
+ footprint_.push_back(pt);
+ }
+
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"), footprint_);
planner_costmap_ros_->getCostmapCopy(planner_costmap_);
@@ -101,7 +135,7 @@
controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &planner_costmap_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
//advertise a service for getting a plan
ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -297,35 +331,6 @@
lock_.unlock();
}
- bool MoveBase::escape(double escape_dist, unsigned int max_attempts, const robot_msgs::PoseStamped& robot_pose){
- ROS_DEBUG("Attempting to generate an escape goal %.2f meters away", escape_dist);
- unsigned int attempts = 0;
- ros::Rate r(controller_frequency_);
- robot_msgs::PoseStamped goal_pose = robot_pose;
- srand(time(0));
- valid_plan_ = false;
- while(!valid_plan_ && attempts < max_attempts){
- double angle = 2 * M_PI * rand() / RAND_MAX;
- double x_diff = escape_dist * cos(angle);
- double y_diff = escape_dist * sin(angle);
- goal_pose = robot_pose;
- goal_pose.pose.position.x += x_diff;
- goal_pose.pose.position.y += y_diff;
- makePlan(goal_pose);
- attempts++;
- r.sleep();
- }
-
- if(valid_plan_){
- ROS_DEBUG("Attempting to escape to point (%.2f, %.2f)", goal_pose.pose.position.x, goal_pose.pose.position.y);
- return true;
- }
-
- ROS_INFO("Could not find a valid escape point");
- return false;
- }
-
-
void MoveBase::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
@@ -408,10 +413,8 @@
}
}
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_costmap_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
+ //compute velocity commands to send to the base
+ valid_control = tc_->computeVelocityCommands(cmd_vel);
ROS_DEBUG("Velocity commands produced by controller: vx: %.2f, vy: %.2f, vth: %.2f", cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.ang_vel.vz);
if(valid_control)
Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -92,7 +92,7 @@
//footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_, &controller_costmap_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
}
MoveBaseLocal::~MoveBaseLocal(){
@@ -197,11 +197,10 @@
}
bool valid_control = false;
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_costmap_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(cmd_vel, observations, false);
+ //compute veloctiy commands to send to the base... don't prune the path
+ valid_control = tc_->computeVelocityCommands(cmd_vel, false);
+
//give the base the velocity command
ros_node_.publish("cmd_vel", cmd_vel);
Modified: pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base_stage/move_base/base_local_planner_params.yaml 2009-06-09 01:14:54 UTC (rev 16847)
@@ -1,3 +1,4 @@
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.325, -0.325]]
base_local_planner:
#Independent settings for the local costmap
costmap:
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch 2009-06-09 01:14:54 UTC (rev 16847)
@@ -4,7 +4,7 @@
<param name="/use_sim_time" value="true"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
- <param name="footprint_padding" value="0.02" />
+ <param name="footprint_padding" value="0.01" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
<rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/costmap_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -72,15 +72,6 @@
virtual double footprintCost(const robot_msgs::Point& position, const std::vector<robot_msgs::Point>& footprint,
double inscribed_radius, double circumscribed_radius);
- /**
- * @brief The costmap already keeps track of world observations, so for this world model this method does nothing
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scans The scans used to clear freespace
- */
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans) {}
-
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/point_grid.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -99,7 +99,7 @@
* @param observations The observations from various sensors
* @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
*/
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
+ void updateWorld(const std::vector<robot_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
/**
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -131,14 +131,10 @@
* @param global_pose The current pose of the robot in world space
* @param global_vel The current velocity of the robot in world space
* @param drive_velocities Will be set to velocities to send to the robot base
- * @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
- * @param laser_scans The latest scans taken... used to clear freespace in front of the robot depending on the model
* @return The selected path or trajectory
*/
Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities,
- std::vector<costmap_2d::Observation> observations = std::vector<costmap_2d::Observation>(0),
- std::vector<PlanarLaserScan> laser_scans = std::vector<PlanarLaserScan>(0));
+ tf::Stamped<tf::Pose>& drive_velocities);
/**
* @brief Update the plan that the controller is following
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -37,9 +37,10 @@
#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
-#include <ros/node.h>
+#include <ros/ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_publisher.h>
+#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/point_grid.h>
#include <base_local_planner/costmap_model.h>
@@ -79,11 +80,9 @@
* @param tf A reference to a transform listener
* @param costmap The cost map to use for assigning costs to trajectories
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
- * @param planner_map Used to size the map for the freespace controller... this will go away once a rolling window version of the point grid is in place
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec,
- const costmap_2d::Costmap2D* planner_map = NULL);
+ costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec);
/**
* @brief Destructor for the wrapper
@@ -106,8 +105,7 @@
* @param prune_plan Set to true if you would like the plan to be pruned as the robot drives, false to leave the plan as is
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
- const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0), bool prune_plan = true);
+ bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan = true);
/**
* @brief Update the plan that the controller is following
@@ -211,23 +209,14 @@
*/
void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
- void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
- void tiltScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void odomCallback();
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2D& costmap_; ///< @brief The costmap the controller will use
- tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
- tf::MessageNotifier<laser_scan::LaserScan>* tilt_scan_notifier_; ///< @brief Used to guarantee that a transform is available for tilt scans
tf::TransformListener& tf_; ///< @brief Used for transforming point clouds
ros::Node& ros_node_; ///< @brief The ros node we're running under
std::string global_frame_; ///< @brief The frame in which the controller will run
- laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
- boost::recursive_mutex obs_lock_; ///< @brief Lock for accessing data in callbacks safely
- std::vector<PlanarLaserScan> laser_scans_; ///< @breif Storage for the last scan the lasers took... used for clearing free-space in front of the robot
- PointGrid* point_grid_; ///< @brief If using a freespace grid... we want to access it
- VoxelGridModel* voxel_grid_; ///< @brief If using a voxel grid... we want to access it
double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
deprecated_msgs::RobotBase2DOdom odom_msg_, base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/voxel_grid_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -93,7 +93,7 @@
* @param observations The observations from various sensors
* @param laser_scan The scans used to clear freespace
*/
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
+ void updateWorld(const std::vector<robot_msgs::Point>& footprint,
const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
void getPoints(robot_msgs::PointCloud& cloud);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/world_model.h 2009-06-09 01:14:54 UTC (rev 16847)
@@ -51,15 +51,6 @@
class WorldModel{
public:
/**
- * @brief Subclass will implement this method to insert observations from sensors into its world model
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scans The planar laser scans used to clear freespace
- */
- virtual void updateWorld(const std::vector<robot_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans) = 0;
-
- /**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
* @param position The position of the robot in world coordinates
* @param footprint The specification of the footprint of the robot in world coordinates
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -471,7 +471,8 @@
double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
Trajectory t;
double impossible_cost = map_.map_.size();
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, t);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, t);
//if the trajectory is a legal one... the check passes
if(t.cost_ >= 0)
@@ -482,7 +483,8 @@
}
//create the trajectories we wish to score
- Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
+ Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
+ double vx, double vy, double vtheta,
double acc_x, double acc_y, double acc_theta){
//compute feasible velocity limits in robot space
double max_vel_x, max_vel_theta;
@@ -531,7 +533,8 @@
for(int i = 0; i < vx_samples_; ++i){
vtheta_samp = 0;
//first sample the straight trajectory
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -543,7 +546,8 @@
vtheta_samp = min_vel_theta;
//next sample all theta trajectories
for(int j = 0; j < vtheta_samples_ - 1; ++j){
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -562,7 +566,8 @@
vx_samp = 0.1;
vy_samp = 0.1;
vtheta_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -574,7 +579,8 @@
vx_samp = 0.1;
vy_samp = -0.1;
vtheta_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
@@ -595,12 +601,17 @@
for(int i = 0; i < vtheta_samples_; ++i){
//enforce a minimum rotational velocity because the base can't handle small in-place rotations
- double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_) : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
+ double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
+ : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
- //if the new trajectory is better... let's take it... note if we can legally rotate in place we prefer to do that rather than move with y velocity
- if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0) && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
+ //if the new trajectory is better... let's take it...
+ //note if we can legally rotate in place we prefer to do that rather than move with y velocity
+ if(comp_traj->cost_ >= 0
+ && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
+ && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
double x_r, y_r, th_r;
comp_traj->getEndpoint(x_r, y_r, th_r);
x_r += heading_lookahead_ * cos(th_r);
@@ -698,7 +709,8 @@
vtheta_samp = 0;
vy_samp = y_vels_[i];
//sample completely horizontal trajectories
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
@@ -790,7 +802,8 @@
vtheta_samp = 0.0;
vx_samp = -0.1;
vy_samp = 0.0;
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp, acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
+ generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
+ acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
/*
@@ -843,8 +856,7 @@
//given the current state of the robot, find a good trajectory
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities, vector<costmap_2d::Observation> observations,
- vector<PlanarLaserScan> laser_scans){
+ tf::Stamped<tf::Pose>& drive_velocities){
double uselessPitch, uselessRoll, yaw, velYaw;
global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
@@ -858,20 +870,6 @@
double vy = global_vel.getOrigin().getY();
double vtheta = velYaw;
- //build the oriented footprint at the robot's current location
- double cos_th = cos(theta);
- double sin_th = sin(theta);
- vector<Point> oriented_footprint;
- for(unsigned int i = 0; i < footprint_spec_.size(); ++i){
- Point new_pt;
- new_pt.x = x + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
- new_pt.y = y + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
- oriented_footprint.push_back(new_pt);
- }
-
- //update the point grid with new observations
- world_model_.updateWorld(oriented_footprint, observations, laser_scans);
-
//reset the map for new operations
map_.resetPathDist();
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:14:38 UTC (rev 16846)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:14:54 UTC (rev 16847)
@@ -48,9 +48,9 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- Costmap2D& costmap, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
- : world_model_(NULL), tc_(NULL), costmap_(costmap), base_scan_notifier_(NULL), tf_(tf), ros_node_(ros_node), laser_scans_(2),
- point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true), costmap_publisher_(NULL){
+ Costmap2D& costmap, std::vector<Point> footprint_spec)
+ : world_model_(NULL), tc_(NULL), costmap_(costmap), tf_(tf), ros_node_(ros_node),
+ rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true), costmap_publisher_(NULL){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_s...
[truncated message content] |
|
From: <ei...@us...> - 2009-06-09 02:22:15
|
Revision: 16850
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16850&view=rev
Author: eitanme
Date: 2009-06-09 01:36:55 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
Moving some logic out of the move_base action TrajectoryPlannerROS now holds a reference to a Costmap2DROS object instead of a Costmap2D obect
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 01:36:55 UTC (rev 16850)
@@ -123,7 +123,7 @@
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
- costmap_2d::Costmap2D planner_costmap_, controller_costmap_;
+ costmap_2d::Costmap2D planner_costmap_;
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -132,10 +132,9 @@
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"), footprint_);
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
//advertise a service for getting a plan
ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -384,7 +383,6 @@
//make sure to update the costmap we'll use for this cycle
controller_costmap_ros_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
if(!controller_costmap_ros_->isCurrent()){
Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -65,7 +65,6 @@
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
controller_costmap_ros_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//initially we'll stop all updates on the costmap
@@ -92,7 +91,7 @@
//footprint_.push_back(pt);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
}
MoveBaseLocal::~MoveBaseLocal(){
@@ -179,9 +178,6 @@
//push the feedback out
update(feedback);
- //make sure to update the costmap we'll use for this cycle
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
-
robot_msgs::PoseDot cmd_vel;
//check that the observation buffers for the costmap are current
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-09 01:36:55 UTC (rev 16850)
@@ -82,7 +82,7 @@
* @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- costmap_2d::Costmap2D& costmap, std::vector<robot_msgs::Point> footprint_spec);
+ costmap_2d::Costmap2DROS& costmap_ros, std::vector<robot_msgs::Point> footprint_spec);
/**
* @brief Destructor for the wrapper
@@ -128,12 +128,6 @@
*/
bool goalReached();
- /**
- * @brief Clear the footprint of the robot in a given cost map
- * @param costmap The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(costmap_2d::Costmap2D& costmap);
-
private:
/**
* @brief Check whether the robot is stopped or not
@@ -198,13 +192,6 @@
void publishFootprint(const tf::Stamped<tf::Pose>& global_pose);
/**
- * @brief Clear the footprint of the robot in a given cost map
- * @param global_pose The pose of the robot in the global frame
- * @param costmap The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& costmap);
-
- /**
* @brief Publish a plan for visualization purposes
*/
void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
@@ -213,7 +200,8 @@
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
- costmap_2d::Costmap2D& costmap_; ///< @brief The costmap the controller will use
+ costmap_2d::Costmap2DROS& costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
+ costmap_2d::Costmap2D costmap_; ///< @brief The costmap the controller will use
tf::TransformListener& tf_; ///< @brief Used for transforming point clouds
ros::Node& ros_node_; ///< @brief The ros node we're running under
std::string global_frame_; ///< @brief The frame in which the controller will run
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -48,8 +48,8 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- Costmap2D& costmap, std::vector<Point> footprint_spec)
- : world_model_(NULL), tc_(NULL), costmap_(costmap), tf_(tf), ros_node_(ros_node),
+ Costmap2DROS& costmap_ros, std::vector<Point> footprint_spec)
+ : world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf), ros_node_(ros_node),
rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true), costmap_publisher_(NULL){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_samples, vtheta_samples;
@@ -59,6 +59,9 @@
double max_vel_x, min_vel_x, max_vel_th, min_vel_th;
string world_model_type;
+ //initialize the copy of the costmap the controller will use
+ costmap_ros_.getCostmapCopy(costmap_);
+
//adverstise the fact that we'll publish the robot footprint
ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/robot_footprint", 1);
ros_node.advertise<visualization_msgs::Polyline>("~base_local_planner/global_plan", 1);
@@ -128,9 +131,9 @@
ros_node.param("~base_local_planner/point_grid/grid_resolution", grid_resolution, 0.2);
ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
- world_model_ = new CostmapModel(costmap);
+ world_model_ = new CostmapModel(costmap_);
- tc_ = new TrajectoryPlanner(*world_model_, costmap, footprint_spec, inscribed_radius_, circumscribed_radius_,
+ tc_ = new TrajectoryPlanner(*world_model_, costmap_, footprint_spec, inscribed_radius_, circumscribed_radius_,
acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_,
@@ -338,8 +341,11 @@
//we also want to clear the robot footprint from the costmap we're using
- clearRobotFootprint(global_pose, costmap_);
+ costmap_ros_.clearRobotFootprint();
+ //make sure to update the costmap we'll use for this cycle
+ costmap_ros_.getCostmapCopy(costmap_);
+
//after clearing the footprint... we want to push the changes to the costmap publisher
if(costmap_publisher_->active())
costmap_publisher_->updateCostmapData(costmap_);
@@ -481,60 +487,6 @@
}
}
- void TrajectoryPlannerROS::clearRobotFootprint(Costmap2D& costmap){
- tf::Stamped<tf::Pose> robot_pose, global_pose;
- robot_pose.setIdentity();
- robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
- ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
-
- //get the global pose of the robot
- try{
- tf_.transformPose(global_frame_, robot_pose, global_pose);
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return;
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return;
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- return;
- }
- // check global_pose timeout
- if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
- ROS_ERROR("TrajcetoryPlannerROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
- current_time.toSec() ,global_pose.stamp_.toSec() ,transform_tolerance_);
- return;
- }
-
- clearRobotFootprint(global_pose, costmap);
- }
-
- void TrajectoryPlannerROS::clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, Costmap2D& costmap){
- double useless_pitch, useless_roll, yaw;
- global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
-
- //get the oriented footprint of the robot
- std::vector<robot_msgs::Point> oriented_footprint = drawFootprint(global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw);
-
- //set the associated costs in the cost map to be free
- if(!costmap.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
- return;
-
- double max_inflation_dist = inflation_radius_ + circumscribed_radius_;
-
- //clear all non-lethal obstacles out to the maximum inflation distance of an obstacle in the robot footprint
- costmap.clearNonLethal(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
-
- //make sure to re-inflate obstacles in the affected region... plus those obstalces that could
- costmap.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist + inflation_radius_, max_inflation_dist + inflation_radius_, false);
-
- }
-
void TrajectoryPlannerROS::publishFootprint(const tf::Stamped<tf::Pose>& global_pose){
double useless_pitch, useless_roll, yaw;
global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-06-09 01:31:54 UTC (rev 16849)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-06-09 01:36:55 UTC (rev 16850)
@@ -100,10 +100,9 @@
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"), footprint_);
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, controller_costmap_, footprint_);
+ tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
//initially clear any unknown space around the robot
planner_costmap_ros_->clearNonLethalWindow(circumscribed_radius_ * 2, circumscribed_radius_ * 2);
@@ -239,7 +238,6 @@
//make sure to update the costmap we'll use for this cycle
controller_costmap_ros_->clearRobotFootprint();
- controller_costmap_ros_->getCostmapCopy(controller_costmap_);
//check that the observation buffers for the costmap are current
if(!controller_costmap_ros_->isCurrent()){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-09 05:44:53
|
Revision: 16854
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16854&view=rev
Author: isucan
Date: 2009-06-09 05:43:35 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
documentation + better output handling
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/world_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODESimple.h
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/output.h
pkg/trunk/motion_planning/planning_models/src/output.cpp
pkg/trunk/world_models/collision_space/src/bodies.cpp
pkg/trunk/world_models/collision_space/src/environment.cpp
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/collision_space/src/environmentODESimple.cpp
Removed Paths:
-------------
pkg/trunk/world_models/collision_space/include/collision_space/output.h
pkg/trunk/world_models/collision_space/src/collision_space/
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_models.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -43,7 +43,7 @@
namespace planning_environment
{
- /** A class capable of loading a robot model from the parameter server */
+ /** \brief A class capable of loading a robot model from the parameter server */
class CollisionModels : public RobotModels
{
@@ -58,7 +58,7 @@
{
}
- /** Reload the robot description and recreate the model */
+ /** \brief Reload the robot description and recreate the model */
virtual void reload(void)
{
RobotModels::reload();
@@ -66,7 +66,7 @@
loadCollision();
}
- /** Return the instance of the constructed ODE collision model */
+ /** \brief Return the instance of the constructed ODE collision model */
const boost::shared_ptr<collision_space::EnvironmentModel> &getODECollisionModel(void) const
{
return ode_collision_model_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -78,37 +78,45 @@
return collisionSpace_;
}
+ /** \brief Return the instance of collision models that is being used */
CollisionModels* getCollisionModels(void) const
{
return cm_;
}
- /** Define a callback for before updating a map */
+ /** \brief Define a callback for before updating a map */
void setOnBeforeMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
{
onBeforeMapUpdate_ = callback;
}
- /** Define a callback for after updating a map */
+ /** \brief Define a callback for after updating a map */
void setOnAfterMapUpdateCallback(const boost::function<void(const robot_msgs::CollisionMapConstPtr)> &callback)
{
onAfterMapUpdate_ = callback;
}
- /** Define a callback for after updating a map */
+ /** \brief Define a callback for after updating a map */
void setOnAfterAttachBodyCallback(const boost::function<void(planning_models::KinematicModel::Link*)> &callback)
{
onAfterAttachBody_ = callback;
}
+ /** \brief Return true if map has been received */
bool haveMap(void) const
{
return haveMap_;
}
- /** Return true if a map update has been received in the last sec seconds */
+ /** \brief Return true if a map update has been received in the last sec seconds */
bool isMapUpdated(double sec) const;
+ /** \brief Return the last update time for the map */
+ const ros::Time& lastMapUpdate(void) const
+ {
+ return lastMapUpdate_;
+ }
+
protected:
void setupCSM(void);
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -77,71 +77,73 @@
delete robotState_;
}
- /** Define a callback for when the state is changed */
+ /** \brief Define a callback for when the state is changed */
void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
{
onStateUpdate_ = callback;
}
+ /** \brief Get the kinematic model that is being monitored */
planning_models::KinematicModel* getKinematicModel(void) const
{
return kmodel_;
}
-
+
+ /** \brief Get the instance of @b RobotModels that is being used */
RobotModels* getRobotModels(void) const
{
return rm_;
}
- /** Return a pointer to the maintained robot state */
+ /** \brief Return a pointer to the maintained robot state */
const planning_models::KinematicModel::StateParams* getRobotState(void) const
{
return robotState_;
}
- /** Return the frame id of the state */
+ /** \brief Return the frame id of the state */
const std::string& getFrameId(void) const
{
return frame_id_;
}
- /** Return true if a pose has been received */
+ /** \brief Return true if a pose has been received */
bool havePose(void) const
{
return havePose_;
}
- /** Return true if a full mechanism state has been received */
+ /** \brief Return true if a full mechanism state has been received */
bool haveState(void) const
{
return haveMechanismState_;
}
- /** Return the time of the last pose update */
+ /** \brief Return the time of the last pose update */
const ros::Time& lastPoseUpdate(void) const
{
return lastPoseUpdate_;
}
- /** Return the time of the last state update */
+ /** \brief Return the time of the last state update */
const ros::Time& lastStateUpdate(void) const
{
return lastStateUpdate_;
}
- /** Wait until a pose is received */
+ /** \brief Wait until a pose is received */
void waitForPose(void) const;
- /** Wait until a full mechanism state is received */
+ /** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** Return true if a pose has been received in the last sec seconds */
+ /** \brief Return true if a pose has been received in the last sec seconds */
bool isPoseUpdated(double sec) const;
- /** Return true if a full mechanism state has been received in the last sec seconds */
+ /** \brief Return true if a full mechanism state has been received in the last sec seconds */
bool isStateUpdated(double sec) const;
- /** Output the current state as ROS INFO */
+ /** \brief Output the current state as ROS INFO */
void printRobotState(void) const;
protected:
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -49,13 +49,13 @@
namespace planning_environment
{
- /** A class capable of loading a robot model from the parameter server */
+ /** \brief A class capable of loading a robot model from the parameter server */
class RobotModels
{
public:
- /** A class to define a planner configuration */
+ /** \brief A class to define a planner configuration */
class PlannerConfig
{
public:
@@ -89,54 +89,67 @@
{
}
+ /** \brief Return the name of the description */
const std::string &getDescription(void) const
{
return description_;
}
- /** Return the instance of the constructed kinematic model */
+ /** \brief Return the instance of the constructed kinematic model */
const boost::shared_ptr<planning_models::KinematicModel> &getKinematicModel(void) const
{
return kmodel_;
}
- /** Return the instance of the parsed robot description */
+ /** \brief Return the instance of the parsed robot description */
const boost::shared_ptr<robot_desc::URDF> &getParsedDescription(void) const
{
return urdf_;
}
+ /** \brief Return the map of the planning groups (arrays of link names) */
const std::map< std::string, std::vector<std::string> > &getPlanningGroups(void) const
{
return planning_groups_;
}
+ /** \brief Return the names of the links that should be considered when performing collision checking */
const std::vector<std::string> &getCollisionCheckLinks(void) const
{
return collision_check_links_;
}
-
+
+ /** \brief Return the names of the links that should be considered when cleaning sensor data
+ of parts the robot can see from itself */
const std::vector<std::string> &getSelfSeeLinks(void) const
{
return self_see_links_;
}
+ /** \brief Return the groups of links that should be considered when testing for self collision. This is an
+ array of pairs. Both elements of the pair are groups of links. If any link in the first member of the pair
+ collides with some link in the second member of the pair, we have a collision */
const std::vector< std::pair < std::vector<std::string>, std::vector<std::string> > > &getSelfCollisionGroups(void) const
{
return self_collision_check_groups_;
}
+ /** \brief Return true if models have been loaded */
bool loadedModels(void) const
{
return loaded_models_;
}
+ /** \brief Get the amount of padding to be used for links when cleaning sensor data */
double getSelfSeePadding(void);
+
+ /** \brief Get the amount of scaling to be used for links when cleaning sensor data */
double getSelfSeeScale(void);
+ /** \breif Get the list of planner configurations available for a specific planning group */
std::vector< boost::shared_ptr<PlannerConfig> > getGroupPlannersConfig(const std::string &group);
- /** Reload the robot description and recreate the model */
+ /** \brief Reload the robot description and recreate the model */
virtual void reload(void);
protected:
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-06-09 05:43:35 UTC (rev 16854)
@@ -2,7 +2,10 @@
\mainpage
\htmlinclude manifest.html
-\b planning_environment is ...
+\b planning_environment is a library that allows users to instantiate
+robot models and collision models based on data from the parameter
+server with minimal user input. Additionally, state information for
+both robot models and collision environments can be monitored.
<!--
In addition to providing an overview of your package,
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -54,7 +54,13 @@
collisionSpace_ = cm_->getODECollisionModel().get();
collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
- attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+ ROS_DEBUG("Listening to collision_map");
+
+ if (cm_->loadedModels())
+ {
+ attachBodySubscriber_ = nh_.subscribe("attach_object", 1, &CollisionSpaceMonitor::attachObjectCallback, this);
+ ROS_DEBUG("Listening to attach_object");
+ }
}
bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
@@ -94,7 +100,7 @@
double tupd = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG("Updated map model in %f seconds", tupd);
- lastMapUpdate_ = ros::Time::now();
+ lastMapUpdate_ = collisionMap->header.stamp;
haveMap_ = true;
if (onAfterMapUpdate_ != NULL)
@@ -139,7 +145,7 @@
// update the collision model
collisionSpace_->updateAttachedBodies();
- ROS_INFO("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
+ ROS_DEBUG("Link '%s' on '%s' has %d objects attached", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str(), n);
}
else
ROS_WARN("Unable to attach object to link '%s' on '%s'", attachedObject->link_name.c_str(), attachedObject->robot_name.c_str());
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -60,11 +60,18 @@
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
if (includePose_)
+ {
localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
+ ROS_DEBUG("Listening to localized pose");
+ }
else
+ {
frame_id_ = kmodel_->getRobot(0)->chain->after->name;
+ ROS_DEBUG("Robot state frame is %s", frame_id_.c_str());
+ }
}
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
+ ROS_DEBUG("Listening to mechanism state");
}
}
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -35,9 +35,54 @@
/** \author Ioan Sucan */
#include "planning_environment/robot_models.h"
+#include <planning_models/output.h>
#include <ros/console.h>
#include <sstream>
+// make sure messages from planning_environments & collision_space go to ROS console
+namespace planning_environment
+{
+ class OutputHandlerROScon : public planning_models::msg::OutputHandler
+ {
+ public:
+
+ OutputHandlerROScon(void) : OutputHandler()
+ {
+ planning_models::msg::useOutputHandler(this);
+ }
+
+ ~OutputHandlerROScon(void)
+ {
+ planning_models::msg::noOutputHandler();
+ }
+
+ /** Issue a ROS error */
+ virtual void error(const std::string &text)
+ {
+ ROS_ERROR("%s", text.c_str());
+ }
+
+ /** Issue a ROS warning */
+ virtual void warn(const std::string &text)
+ {
+ ROS_WARN("%s", text.c_str());
+ }
+
+ /** Issue ROS info */
+ virtual void inform(const std::string &text)
+ {
+ ROS_INFO("%s", text.c_str());
+ }
+
+ /** Issue ROS debug */
+ virtual void message(const std::string &text)
+ {
+ ROS_DEBUG("%s", text.c_str());
+ }
+ };
+ static OutputHandlerROScon _outputHandler;
+}
+
void planning_environment::RobotModels::reload(void)
{
kmodel_.reset();
@@ -74,7 +119,10 @@
getSelfSeeLinks(self_see_links_);
}
else
+ {
urdf_.reset();
+ ROS_ERROR("Unable to parse URDF description!");
+ }
}
else
ROS_ERROR("Robot model '%s' not found!", description_.c_str());
Modified: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.xml 2009-06-09 05:43:35 UTC (rev 16854)
@@ -1,7 +1,7 @@
<launch>
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find planning_environment)/planning.yaml" />
+ <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find pr2_defs)/planning/planning.yaml" />
<test test-name="test_robot_models" pkg="planning_environment" type="test_robot_models" />
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-06-09 05:43:35 UTC (rev 16854)
@@ -6,7 +6,8 @@
set(ROS_BUILD_TYPE Debug)
rospack_add_library(planning_models src/kinematic.cpp
- src/load_mesh.cpp)
+ src/load_mesh.cpp
+ src/output.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -38,7 +38,7 @@
#define PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
#include "planning_models/shapes.h"
-
+#include "planning_models/output.h"
#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <boost/thread/mutex.hpp>
@@ -49,12 +49,8 @@
#include <map>
#include <cassert>
-/** @htmlinclude ../../manifest.html
+/** Describing a kinematic robot model loaded from URDF. Visual geometry is ignored */
- @mainpage
-
- A class describing a kinematic robot model loaded from URDF. Visual geometry is ignored */
-
/** Main namespace */
namespace planning_models
{
@@ -551,6 +547,7 @@
protected:
KinematicModel *m_owner;
+ msg::Interface m_msg;
ModelInfo &m_mi;
double *m_params;
std::map<unsigned int, bool> m_seen;
@@ -653,7 +650,8 @@
bool m_built;
boost::mutex m_lock;
-
+ msg::Interface m_msg;
+
private:
/** Build the needed datastructure for a joint */
Added: pkg/trunk/motion_planning/planning_models/include/planning_models/output.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/output.h (rev 0)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/output.h 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,133 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#ifndef PLANNING_MODELS_OUTPUT_INTERFACE_
+#define PLANNING_MODELS_OUTPUT_INTERFACE_
+
+#include <string>
+#include <cstdarg>
+
+/** Main namespace */
+namespace planning_models
+{
+
+ /** Message namespace */
+ namespace msg
+ {
+
+ /** The piece of code that desires interaction with an action
+ or an output handler should use an instance of this
+ class */
+ class Interface
+ {
+ public:
+
+ Interface(void);
+ virtual ~Interface(void);
+
+ void inform(const std::string &text) const;
+ void warn(const std::string &text) const;
+ void error(const std::string &text) const;
+ void message(const std::string &text) const;
+
+ void inform(const char *msg, ...) const;
+ void warn(const char *msg, ...) const;
+ void error(const char *msg, ...) const;
+ void message(const char *msg, ...) const;
+
+ protected:
+
+ std::string combine(const char *msg, va_list ap) const;
+
+ };
+
+ /** Generic class to handle output from a piece of code */
+ class OutputHandler
+ {
+ public:
+
+ OutputHandler(void)
+ {
+ }
+
+ virtual ~OutputHandler(void)
+ {
+ }
+
+ /** Print an error message: "Error: ...." */
+ virtual void error(const std::string &text) = 0;
+
+ /** Print an warning message: "Warning: ...." */
+ virtual void warn(const std::string &text) = 0;
+
+ /** Print some information: "Information: ...." */
+ virtual void inform(const std::string &text) = 0;
+
+ /** Print a simple message */
+ virtual void message(const std::string &text) = 0;
+ };
+
+ class OutputHandlerSTD : public OutputHandler
+ {
+ public:
+
+ OutputHandlerSTD(void) : OutputHandler()
+ {
+ }
+
+ /** Print an error message: "Error: ...." */
+ virtual void error(const std::string &text);
+
+ /** Print an warning message: "Warning: ...." */
+ virtual void warn(const std::string &text);
+
+ /** Print some information: "Information: ...." */
+ virtual void inform(const std::string &text);
+
+ /** Print a simple message */
+ virtual void message(const std::string &text);
+
+
+ };
+
+ void noOutputHandler(void);
+ void useOutputHandler(OutputHandler *oh);
+ OutputHandler* getOutputHandler(void);
+ }
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-09 03:59:59 UTC (rev 16853)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -36,6 +36,7 @@
#include <planning_models/kinematic.h>
#include <algorithm>
+#include <sstream>
#include <cmath>
namespace planning_models
@@ -378,7 +379,7 @@
{
if (m_built)
{
- std::cerr << "Model has already been built!" << std::endl;
+ m_msg.error("Model has already been built!");
return;
}
@@ -555,11 +556,11 @@
if (m_verbose && joint->usedParams > 0)
{
- std::cout << "Joint '" << urdfLink->joint->name << "' connects link '" << urdfLink->parentName << "' to link '" <<
- urdfLink->name << "' and uses state coordinates: ";
+ m_msg.message("Joint '" + urdfLink->joint->name + "' connects link '" + urdfLink->parentName + "' to link '" + urdfLink->name + "' and uses state coordinates: ");
+ std::stringstream ss;
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- std::cout << i + robot->stateDimension << " ";
- std::cout << std::endl;
+ ss << i + robot->stateDimension << " ";
+ m_msg.message(ss.str());
}
robot->stateDimension += joint->usedParams;
@@ -588,6 +589,7 @@
planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(const robot_desc::URDF::Link* urdfLink)
{
Joint *newJoint = NULL;
+ std::stringstream ss;
switch (urdfLink->joint->type)
{
case robot_desc::URDF::Link::Joint::FIXED:
@@ -606,7 +608,8 @@
newJoint = new RevoluteJoint();
break;
default:
- std::cerr << "Unknown joint type " << urdfLink->joint->type << std::endl;
+ ss << urdfLink->joint->type;
+ m_msg.error("Unknown joint type " + ss.str());
break;
}
return newJoint;
@@ -744,7 +747,7 @@
}
}
else
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
return result;
}
@@ -878,10 +881,12 @@
for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
if (m_mi.groupStateIndexList[groupID][i] == pos)
return i;
- std::cerr << "Joint '" << name << "' is not in group " << groupID << std::endl;
+ std::stringstream ss;
+ ss << groupID;
+ m_msg.error("Joint '" + name + "' is not in group " + ss.str());
}
}
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
return -1;
}
@@ -899,7 +904,7 @@
return;
}
}
- std::cerr << "Unknown joint: '" << name << "'" << std::endl;
+ m_msg.error("Unknown joint: '" + name + "'");
}
void planning_models::KinematicModel::StateParams::copyParams(double *params) const
Added: pkg/trunk/motion_planning/planning_models/src/output.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/output.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_models/src/output.cpp 2009-06-09 05:43:35 UTC (rev 16854)
@@ -0,0 +1,176 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include "planning_models/output.h"
+#include <boost/thread/mutex.hpp>
+#include <iostream>
+#include <cstdlib>
+
+static planning_models::msg::OutputHandlerSTD _defaultOutputHandler;
+static planning_models:...
[truncated message content] |
|
From: <hsu...@us...> - 2009-06-09 17:18:52
|
Revision: 16862
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16862&view=rev
Author: hsujohnhsu
Date: 2009-06-09 17:18:45 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
balcony world.
Added Paths:
-----------
pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/balcony.world
Added: pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch (rev 0)
+++ pkg/trunk/demos/pr2_gazebo/pr2_balcony.launch 2009-06-09 17:18:45 UTC (rev 16862)
@@ -0,0 +1,34 @@
+<launch>
+ <!-- send urdf to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
+
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/balcony.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 5 5 0 0 0 0" respawn="false" output="screen" />
+
+ <!-- startup base controller -->
+ <param name="base_controller/odom_publish_rate" value="10" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
+ <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/gazebo_head_torso_lift_controller.xml" output="screen"/>
+
+ <!-- for visualization -->
+ <node pkg="rviz" type="rviz" respawn="false" />
+
+ <!-- dynamic map generation -->
+ <node pkg="slam_gmapping" type="slam_gmapping" respawn="false" />
+
+ <!-- nav-stack:
+ NOTE: not working with amcl in loop, need to fix how amcl updates dynamic map
+ NOTE: for now, rosrun teleop_base teleop_base_keyboard to drive robot around
+ -->
+ <!--include file="$(find 2dnav_gazebo)/2dnav-stack-amcl.launch"/-->
+ <!--node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/Media/materials/textures/map_blank.png 0.1" respawn="true" /-->
+
+</launch>
+
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/balcony.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/balcony.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/balcony.world 2009-06-09 17:18:45 UTC (rev 16862)
@@ -0,0 +1,335 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+<!-- cfm is 1e-5 for single precision -->
+<!-- erp is typically .1-.8 -->
+<!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.000000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>10</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ </physics:ode>
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>0 0 20</xyz>
+ <rpy>0 90 90</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>10</maxUpdateRate>
+ </rendering:ogre>
+
+ <!-- ground plane
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <material>PR2/floor_texture</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+ -->
+
+ <!-- floors -->
+ <model:physical name="floor_1_model">
+ <xyz>5 5 -0.1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="floor_1_body">
+ <geom:box name="floor_1_geom">
+ <mesh>default</mesh>
+ <size>10 10 .2</size>
+ <visual>
+ <size>10 10 .2</size>
+ <material>PR2/White</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="floor_2_model">
+ <xyz>5 22.5 5.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="floor_2_body">
+ <geom:box name="floor_2_geom">
+ <mesh>default</mesh>
+ <size>10 5 .2</size>
+ <visual>
+ <size>10 5 .2</size>
+ <material>PR2/White</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="floor_3_model">
+ <xyz>5 25 -0.1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="floor_3_body">
+ <geom:box name="floor_3_geom">
+ <mesh>default</mesh>
+ <size>10 10 .2</size>
+ <visual>
+ <size>10 10 .2</size>
+ <material>PR2/White</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="ramp_1_model">
+ <xyz>6.5 15 2.45</xyz>
+ <rpy>26.9 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="ramp_1_body">
+ <geom:box name="ramp_1_geom">
+ <mesh>default</mesh>
+ <size>1 11.1803 .2</size>
+ <visual>
+ <size>1 11.1803 .2</size>
+ <material>PR2/White</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+ <!-- office walls -->
+ <model:physical name="wall_1_model">
+ <xyz>5 0 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_1_body">
+ <geom:box name="wall_1_geom">
+ <mesh>default</mesh>
+ <size>10 .2 2</size>
+ <visual>
+ <size>10 .2 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="wall_2_model">
+ <xyz>5 20 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_2_body">
+ <geom:box name="wall_2_geom">
+ <mesh>default</mesh>
+ <size>10 .2 2</size>
+ <visual>
+ <size>10 .2 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="wall_7_model">
+ <xyz>5 30 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_7_body">
+ <geom:box name="wall_7_geom">
+ <mesh>default</mesh>
+ <size>10 .2 2</size>
+ <visual>
+ <size>10 .2 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+ <model:physical name="wall_8_model">
+ <xyz>0 25 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_8_body">
+ <geom:box name="wall_8_geom">
+ <mesh>default</mesh>
+ <size>.2 10 2</size>
+ <visual>
+ <size>.2 10 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="wall_9_model">
+ <xyz>10 25 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_9_body">
+ <geom:box name="wall_9_geom">
+ <mesh>default</mesh>
+ <size>.2 10 2</size>
+ <visual>
+ <size>.2 10 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+ <model:physical name="wall_3_model">
+ <xyz>0 5 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_3_body">
+ <geom:box name="wall_3_geom">
+ <mesh>default</mesh>
+ <size>.2 10 2</size>
+ <visual>
+ <size>.2 10 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="wall_4_model">
+ <xyz>10 5 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_4_body">
+ <geom:box name="wall_4_geom">
+ <mesh>default</mesh>
+ <size>.2 10 2</size>
+ <visual>
+ <size>.2 10 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <!-- interior walls -->
+ <model:physical name="wall_5_model">
+ <xyz>3 10 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_5_body">
+ <geom:box name="wall_5_geom">
+ <mesh>default</mesh>
+ <size>6 .2 2</size>
+ <visual>
+ <size>6 .2 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="wall_6_model">
+ <xyz>8.5 10 1</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:box name="wall_6_body">
+ <geom:box name="wall_6_geom">
+ <mesh>default</mesh>
+ <size>3 .2 2</size>
+ <visual>
+ <size>3 .2 2</size>
+ <material>PR2/Green</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+
+
+ <model:empty name="factory_model">
+ <model_thread>false</model_thread>
+ <controller:factory name="factory_controller">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:factory name="factory_iface"/>
+ </controller:factory>
+ </model:empty>
+
+ <!-- White Directional light -->
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ <range>10</range>
+ </light>
+ </model:renderable>
+ <!--
+ -->
+
+
+</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-06-09 17:38:17
|
Revision: 16866
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16866&view=rev
Author: eitanme
Date: 2009-06-09 17:38:10 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
NavfnROS now takes a reference to a Costmap2DROS object instead of a Costmap2D object to move complexity out of the MoveBase action
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-09 17:38:10 UTC (rev 16866)
@@ -123,7 +123,6 @@
bool run_planner_;
base_local_planner::TrajectoryPlannerROS* tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
- costmap_2d::Costmap2D planner_costmap_;
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-09 17:38:10 UTC (rev 16866)
@@ -124,11 +124,10 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"), footprint_);
- planner_costmap_ros_->getCostmapCopy(planner_costmap_);
//initialize the NavFn planner
- planner_ = new NavfnROS(ros_node_, tf_, planner_costmap_);
- ROS_INFO("MAP SIZE: %d, %d", planner_costmap_.cellSizeX(), planner_costmap_.cellSizeY());
+ planner_ = new NavfnROS(ros_node_, tf_, *planner_costmap_ros_);
+ ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->cellSizeX(), planner_costmap_ros_->cellSizeY());
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"), footprint_);
@@ -215,12 +214,7 @@
//update the copy of the costmap the planner uses
clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
- planner_costmap_ros_->clearRobotFootprint();
- planner_costmap_ros_->getCostmapCopy(planner_costmap_);
- //since we have a controller that knows the full footprint of the robot... we may as well clear it
- //tc_->clearRobotFootprint(planner_costmap_); //now done in sensors
-
//if we have a tolerance on the goal point that is greater
//than the resolution of the map... compute the full potential function
double resolution = planner_costmap_ros_->resolution();
@@ -292,13 +286,6 @@
if(planner_costmap_ros_ == NULL)
return;
- //update the copy of the costmap the planner uses
- planner_costmap_ros_->clearRobotFootprint();
- planner_costmap_ros_->getCostmapCopy(planner_costmap_);
-
- //since we have a controller that knows the full footprint of the robot... we may as well clear it
- //tc_->clearRobotFootprint(planner_costmap_); //now done in sensors
-
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-09 17:38:10 UTC (rev 16866)
@@ -39,7 +39,7 @@
#include <ros/node.h>
#include <navfn/navfn.h>
-#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/costmap_2d_ros.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
#include <visualization_msgs/Polyline.h>
@@ -54,9 +54,9 @@
* @brief Constructor for the NavFnROS object
* @param ros_node The a reference to the ros node running
* @param tf A reference to a TransformListener
- * @param cos_map A reference to the costmap to use
+ * @param costmap_ros A reference to the ROS wrapper of the costmap to use
*/
- NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -98,7 +98,8 @@
void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
ros::Node& ros_node_;
tf::TransformListener& tf_;
- costmap_2d::Costmap2D& costmap_;
+ costmap_2d::Costmap2DROS& costmap_ros_;
+ costmap_2d::Costmap2D costmap_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
double transform_tolerance_; // timeout before transform errors
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-09 17:35:35 UTC (rev 16865)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-09 17:38:10 UTC (rev 16866)
@@ -37,8 +37,11 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& costmap) : ros_node_(ros_node), tf_(tf),
- costmap_(costmap), planner_(costmap.cellSizeX(), costmap.cellSizeY()) {
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros) : ros_node_(ros_node), tf_(tf),
+ costmap_ros_(costmap_ros), planner_(costmap_ros.cellSizeX(), costmap_ros.cellSizeY()) {
+ //get an initial copy of the costmap
+ costmap_ros_.getCostmapCopy(costmap_);
+
//advertise our plan visualization
ros_node_.advertise<visualization_msgs::Polyline>("~navfn/plan", 1);
@@ -72,6 +75,10 @@
}
bool NavfnROS::computePotential(const robot_msgs::Point& world_point){
+ //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles
+ costmap_ros_.clearRobotFootprint();
+ costmap_ros_.getCostmapCopy(costmap_);
+
planner_.setCostmap(costmap_.getCharMap());
unsigned int mx, my;
@@ -110,6 +117,10 @@
}
bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
+ //make sure that we have the latest copy of the costmap and that we clear the footprint of obstacles
+ costmap_ros_.clearRobotFootprint();
+ costmap_ros_.getCostmapCopy(costmap_);
+
//until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
if(goal.header.frame_id != global_frame_){
ROS_ERROR("The goal passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-06-09 20:20:53
|
Revision: 16871
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16871&view=rev
Author: vijaypradeep
Date: 2009-06-09 20:19:58 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
Lots of updates to the laser/camera checkboard data pipeline:
- Now publishing left and right pixel locations of cb corners in the stereo checkerboard detector.
- Fixing timestamp issues in the debug images.
Now publishing empty corner vector when checkerboard not found
- Finishing up laser_head grabber.
- Scaling up checkerboards before extracting corners in the mono checkerboard detector. (A hack, but very useful for processing laser scans)
Modified Paths:
--------------
pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h
pkg/trunk/vision/stereo_checkerboard_detector/src/checkerboard_corners_node.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp
pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
Added Paths:
-----------
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
Modified: pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt 2009-06-09 20:15:55 UTC (rev 16870)
+++ pkg/trunk/calibration/kinematic_calibration/CMakeLists.txt 2009-06-09 20:19:58 UTC (rev 16871)
@@ -24,6 +24,11 @@
#target_link_libraries(sensor_kinematics_grabber kinematic_calibration)
#rospack_link_boost(sensor_kinematics_grabber thread)
+rospack_add_executable(laser_head_grabber src/laser_head_grabber.cpp)
+target_link_libraries(laser_head_grabber kinematic_calibration)
+rospack_link_boost(laser_head_grabber thread)
+
+
rospack_add_executable(run_chain_dumper src/run_chain_dumper.cpp)
target_link_libraries(run_chain_dumper kinematic_calibration)
rospack_link_boost(run_chain_dumper thread)
Added: pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-09 20:19:58 UTC (rev 16871)
@@ -0,0 +1,437 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#include <stdio.h>
+
+#include "ros/node.h"
+#include "boost/thread/mutex.hpp"
+#include "robot_msgs/MechanismState.h"
+
+#include "topic_synchronizer/topic_synchronizer.h"
+
+#include "std_msgs/Empty.h"
+#include "robot_msgs/PointCloud.h"
+
+#include "image_msgs/Image.h"
+#include "image_msgs/CamInfo.h"
+
+#include "kinematic_calibration/CalSnapshot.h"
+
+#include <unistd.h>
+#include <termios.h>
+
+using namespace std ;
+
+#define ADD_MSG(type, name) \
+ type name ; \
+ type safe_##name ;
+
+#define TO_SAFE(var) safe_##var = var
+
+namespace kinematic_calibration
+{
+
+
+/**
+ * This node is kind of a hack, currently being used to grab stereo and tilt laser
+ * data for doing the laser head calibration.
+ */
+class LaserHeadGrabber
+{
+
+public:
+
+ ros::Node* node_ ;
+
+ TopicSynchronizer<LaserHeadGrabber> stereo_sync_ ;
+ TopicSynchronizer<LaserHeadGrabber> laser_sync_ ;
+
+ // Define the output message
+ CalSnapshot snapshot_msg_ ;
+
+ // Mechanism State
+ boost::mutex mech_state_lock_ ;
+ ADD_MSG(robot_msgs::MechanismState, mech_state_) ;
+
+ // Stereo Messages
+ boost::mutex stereo_lock_ ;
+ ADD_MSG(robot_msgs::PointCloud, corners_left_) ;
+ ADD_MSG(robot_msgs::PointCloud, corners_right_) ;
+ ADD_MSG(image_msgs::Image, left_debug_) ;
+ ADD_MSG(image_msgs::Image, right_debug_) ;
+ ADD_MSG(image_msgs::CamInfo, left_info_) ;
+ ADD_MSG(image_msgs::CamInfo, right_info_) ;
+
+ // Laser Messages
+ boost::mutex laser_lock_ ;
+ ADD_MSG(robot_msgs::PointCloud, laser_measurement_) ;
+ ADD_MSG(image_msgs::Image, laser_debug_) ;
+
+ // Empty message used for callbacks
+ std_msgs::Empty capture_msg_ ;
+
+ unsigned int capture_count_ ;
+ bool waiting_for_keypress_ ;
+ boost::mutex keypress_lock_ ;
+
+ LaserHeadGrabber(ros::Node* node) : node_(node),
+ stereo_sync_(node_, this, &LaserHeadGrabber::stereoCallback,
+ ros::Duration().fromSec(2.0),
+ &LaserHeadGrabber::stereoTimeout),
+ laser_sync_(node_, this, &LaserHeadGrabber::laserCallback,
+ ros::Duration().fromSec(2.0),
+ &LaserHeadGrabber::laserTimeout)
+
+ {
+ capture_count_ = 0 ;
+ waiting_for_keypress_ = false ;
+
+ node_->advertise<CalSnapshot>("~snapshots", 1) ;
+ node_->advertise<image_msgs::Image>("~left", 1) ;
+ node_->advertise<image_msgs::Image>("~right", 1) ;
+ node_->advertise<image_msgs::Image>("~laser", 1) ;
+
+ node_->subscribe("mechanism_state", mech_state_, &LaserHeadGrabber::mechStateCallback, this, 1) ;
+
+ stereo_sync_.subscribe("stereo/left/cam_info", left_info_, 1) ;
+ stereo_sync_.subscribe("stereo/right/cam_info", right_info_, 1) ;
+ stereo_sync_.subscribe("cb_corners_left", corners_left_, 1) ;
+ stereo_sync_.subscribe("cb_corners_right", corners_right_, 1) ;
+ stereo_sync_.subscribe("left_cb_debug", left_debug_, 1) ;
+ stereo_sync_.subscribe("right_cb_debug", right_debug_, 1) ;
+
+ laser_sync_.subscribe("checkerboard_corners_node/debug_image", laser_debug_, 1) ;
+ laser_sync_.subscribe("/dense_tilt_scan/measured_corners", laser_measurement_, 1) ;
+
+ stereo_sync_.ready() ;
+ laser_sync_.ready() ;
+
+ node_->subscribe("~capture", capture_msg_, &LaserHeadGrabber::captureCallback, this, 1) ;
+ }
+
+ ~LaserHeadGrabber()
+ {
+
+ }
+
+ void captureCallback()
+ {
+/*
+ CalibrationData2 all_data ;
+ printf("\n") ;
+ capture_count_++ ;
+ printf("Capturing Data #%u...\n", capture_count_) ;
+ raw_stereo_lock_.lock() ;
+ all_data.raw_stereo = safe_raw_stereo_ ;
+ raw_stereo_lock_.unlock() ;
+
+ laser_cloud_lock_.lock() ;
+ all_data.laser_cloud = safe_laser_cloud_ ;
+ laser_cloud_lock_.unlock() ;
+
+ mech_state_lock_.lock() ;
+ all_data.mechanism_state = safe_mech_state_ ;
+ mech_state_lock_.unlock() ;
+
+ hi_res_lock_.lock() ;
+ all_data.set_image_size(1) ;
+ all_data.set_cam_info_size(1) ;
+ all_data.image[0] = safe_hi_res_image_ ;
+ all_data.cam_info[0] = safe_hi_res_info_ ;
+ hi_res_lock_.unlock() ;
+
+
+
+ displayAllInfo(all_data) ;
+
+ publish("~calibration_data", all_data) ;*/
+ }
+
+ void mechStateCallback()
+ {
+ mech_state_lock_.lock() ;
+ TO_SAFE(mech_state_) ;
+ mech_state_lock_.unlock() ;
+ }
+
+ bool spin()
+ {
+ // 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);
+
+ bool need_to_quit = false ;
+
+ while (node_->ok() && !need_to_quit)
+ {
+ char c = getchar() ;
+
+ switch (c)
+ {
+ case 'y':
+ case 'Y':
+ {
+ printf("\n") ;
+ triggerCapture(true) ;
+ break ;
+ }
+ case 'n':
+ case 'N':
+ {
+ printf("\n") ;
+ triggerCapture(false) ;
+ break ;
+ }
+ case EOF: // Means that we didn't catch any keyboard hits
+ {
+ usleep(1000) ;
+ break ;
+ }
+ default:
+ {
+ printf("\nPress <Ctrl>-C to Exit\n") ;
+ break ;
+ }
+ }
+ } ;
+
+ tcsetattr(fd,TCSANOW, &prev_flags) ; // Undo any terminal changes that we made
+ return true ;
+ }
+
+
+ void stereoCallback(ros::Time t)
+ {
+ //ROS_INFO("Stereo Callback - %f", t.toSec()) ;
+ stereo_lock_.lock() ;
+
+ TO_SAFE(left_info_) ;
+ TO_SAFE(right_info_) ;
+ TO_SAFE(corners_left_) ;
+ TO_SAFE(corners_right_) ;
+ TO_SAFE(left_debug_) ;
+ TO_SAFE(right_debug_) ;
+
+ stereo_lock_.unlock() ;
+ }
+
+ void stereoTimeout(ros::Time t)
+ {
+ ROS_WARN("Stereo Timeout - %f", t.toSec()) ;
+
+ if (left_info_.header.stamp != t)
+ printf("- left info %f\n", left_info_.header.stamp.toSec()) ;
+ else
+ printf("+ left info %f\n", left_info_.header.stamp.toSec()) ;
+
+ if (right_info_.header.stamp != t)
+ printf("- right info %f\n", right_info_.header.stamp.toSec()) ;
+ else
+ printf("+ right info %f\n", right_info_.header.stamp.toSec()) ;
+
+ if (corners_left_.header.stamp != t)
+ printf("- corners left %f\n", corners_left_.header.stamp.toSec()) ;
+ else
+ printf("+ corners left %f\n", corners_left_.header.stamp.toSec()) ;
+
+ if (corners_right_.header.stamp != t)
+ printf("- corners right %f\n", corners_right_.header.stamp.toSec()) ;
+ else
+ printf("+ corners right %f\n", corners_right_.header.stamp.toSec()) ;
+
+ if (left_debug_.header.stamp != t)
+ printf("- left debug %f\n", left_debug_.header.stamp.toSec()) ;
+ else
+ printf("+ left debug %f\n", left_debug_.header.stamp.toSec()) ;
+
+ if (right_debug_.header.stamp != t)
+ printf("- right debug %f\n", right_debug_.header.stamp.toSec()) ;
+ else
+ printf("+ right debug %f\n", right_debug_.header.stamp.toSec()) ;
+ }
+
+ void laserCallback(ros::Time t)
+ {
+ //ROS_INFO("Laser Callback - %f", t.toSec()) ;
+ laser_lock_.lock() ;
+ TO_SAFE(laser_measurement_) ;
+ TO_SAFE(laser_debug_) ;
+ laser_lock_.unlock() ;
+
+ keypress_lock_.lock() ;
+ if (waiting_for_keypress_)
+ {
+ printf("Throwing away new data!\n");
+ printf("Still waiting for response (Y/N)...\n");
+ }
+ else
+ {
+ buildSnapshotMessage() ;
+ waiting_for_keypress_ = true ;
+ printf("Press Y to save data. Press N to discard...\n") ;
+ }
+ keypress_lock_.unlock() ;
+ }
+
+ void buildSnapshotMessage()
+ {
+ mech_state_lock_.lock() ;
+ stereo_lock_.lock() ;
+ laser_lock_.lock() ;
+ snapshot_msg_.header.stamp = safe_laser_measurement_.header.stamp ;
+ snapshot_msg_.mech_state = safe_mech_state_ ;
+
+ // Build Laser Measurement Vector (Tilt angle, pointing angle, dist)
+ SensorSample laser_sample ;
+ laser_sample.m.resize(safe_laser_measurement_.pts.size()*3) ;
+ for(unsigned int i=0; i<safe_laser_measurement_.pts.size(); i++)
+ {
+ laser_sample.m[3*i+0] = safe_laser_measurement_.pts[i].x ;
+ laser_sample.m[3*i+1] = safe_laser_measurement_.pts[i].y ;
+ laser_sample.m[3*i+2] = safe_laser_measurement_.pts[i].z ;
+ }
+ laser_sample.sensor = "tilt_laser" ;
+ laser_sample.target = "6x8_cb" ;
+
+ SensorSample left_cb_sample = buildCamCornersSample(safe_corners_left_, "stereo_left", "6x8_cb") ;
+ SensorSample right_cb_sample = buildCamCornersSample(safe_corners_right_, "stereo_right", "6x8_cb") ;
+ SensorSample left_info_sample = buildCamInfoSample(safe_left_info_, "stereo_left_info","6x8_cb") ;
+ SensorSample right_info_sample = buildCamInfoSample(safe_right_info_, "stereo_right_info","6x8_cb") ;
+
+ snapshot_msg_.samples.resize(5) ;
+ snapshot_msg_.samples[0] = laser_sample ;
+ snapshot_msg_.samples[1] = left_cb_sample ;
+ snapshot_msg_.samples[2] = right_cb_sample ;
+ snapshot_msg_.samples[3] = left_info_sample ;
+ snapshot_msg_.samples[4] = right_info_sample ;
+
+ node_->publish("~left", safe_left_debug_) ;
+ node_->publish("~right", safe_right_debug_) ;
+ node_->publish("~laser", safe_laser_debug_) ;
+
+ laser_lock_.unlock() ;
+ stereo_lock_.unlock() ;
+ mech_state_lock_.unlock() ;
+ }
+
+ void triggerCapture(bool should_capture)
+ {
+ keypress_lock_.lock() ;
+ if (waiting_for_keypress_)
+ {
+ if (should_capture)
+ {
+ capture_count_++ ;
+ printf("%u - Publishing Sample\n", capture_count_) ;
+ node_->publish("~snapshots", snapshot_msg_) ;
+ }
+ else
+ printf("Throwing away sample\n") ;
+
+ // No longer have fresh data, so we're no longer waiting for a user keypress
+ waiting_for_keypress_ = false ;
+ }
+ else
+ printf("No data needs review yet. Wait until new data is ready...\n") ;
+
+
+ keypress_lock_.unlock() ;
+ }
+
+
+ SensorSample buildCamCornersSample(const robot_msgs::PointCloud& corners, const string& sensor, const string& target)
+ {
+ SensorSample sample ;
+ sample.sensor = sensor ;
+ sample.target = target ;
+ sample.m.resize(2*corners.pts.size()) ;
+ for(unsigned int i=0; i<corners.pts.size(); i++)
+ {
+ sample.m[2*i+0] = corners.pts[i].x ;
+ sample.m[2*i+1] = corners.pts[i].y ;
+ }
+ return sample ;
+ }
+
+ SensorSample buildCamInfoSample(const image_msgs::CamInfo& info, const string& sensor, const string& target)
+ {
+ SensorSample sample ;
+ sample.sensor = sensor ;
+ sample.target = target ;
+ sample.m.resize(info.P.size()) ;
+ for(unsigned int i=0; i<info.P.size(); i++)
+ sample.m[i] = info.P[i] ;
+ return sample ;
+ }
+
+
+ void laserTimeout(ros::Time t)
+ {
+ ROS_WARN("Laser Timeout - %f", t.toSec()) ;
+
+ if (laser_measurement_.header.stamp != t)
+ printf("- Laser Measurement %f\n", laser_measurement_.header.stamp.toSec()) ;
+ else
+ printf("+ Laser Measurement %f\n", laser_measurement_.header.stamp.toSec()) ;
+
+ if (laser_debug_.header.stamp != t)
+ printf("- Laser Debug %f\n", laser_debug_.header.stamp.toSec()) ;
+ else
+ printf("+ Laser Debug %f\n", laser_debug_.header.stamp.toSec()) ;
+ }
+
+} ;
+
+}
+
+using namespace kinematic_calibration ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv) ;
+
+ ros::Node node("laser_head_grabber") ;
+
+ LaserHeadGrabber grabber(&node) ;
+ grabber.spin() ;
+
+ return 0 ;
+}
Modified: pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h 2009-06-09 20:15:55 UTC (rev 16870)
+++ pkg/trunk/vision/stereo_checkerboard_detector/include/stereo_checkerboard_detector/stereo_checkerboard_helper.h 2009-06-09 20:19:58 UTC (rev 16871)
@@ -95,6 +95,18 @@
xyz = xyz_ ;
}
+ void getCornersLeft(std::vector<robot_msgs::Point>& left_xy)
+ {
+ left_xy.clear() ;
+ left_xy = left_xy_ ;
+ }
+
+ void getCornersRight(std::vector<robot_msgs::Point>& right_xy)
+ {
+ right_xy.clear() ;
+ right_xy = right_xy_ ;
+ }
+
void getPose(tf::Pose& pose)
{
pose = pose_ ;
@@ -123,6 +135,10 @@
image_msgs::Image left_ros_debug_ ;
image_msgs::Image right_ros_debug_ ;
+ //! Stores 2D corner locations in pixel coordinates
+ std::vector<robot_msgs::Point> left_xy_ ;
+ std::vector<robot_msgs::Point> right_xy_ ;
+
//! Stores corner locations cartesian coordinates
std::vector<robot_msgs::Point> xyz_ ;
Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/checkerboard_corners_node.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/checkerboard_corners_node.cpp 2009-06-09 20:15:55 UTC (rev 16870)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/checkerboard_corners_node.cpp 2009-06-09 20:19:58 UTC (rev 16871)
@@ -77,24 +77,35 @@
// Convert the ROS Image into openCV's IPL image
IplImage* cv_image = img_bridge_.toIpl() ;
+ const int scale = 2 ;
+ IplImage* cv_image_scaled = cvCreateImage(cvSize(cv_image->width*scale, cv_image->height*scale),
+ cv_image->depth, cv_image->nChannels) ;
+ cvResize(cv_image, cv_image_scaled, CV_INTER_LINEAR);
vector<CvPoint2D32f> cv_corners ;
cv_corners.resize(board_size_.width*board_size_.height) ;
// ******** Checkerboard Extraction **********
//bool found = helper_.getCorners(cv_image, cv_corners) ;
int num_corners ;
- int found = cvFindChessboardCorners( cv_image, board_size_, &cv_corners[0], &num_corners,
- CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE) ;
+ int found = cvFindChessboardCorners( cv_image_scaled, board_size_, &cv_corners[0], &num_corners,
+ CV_CALIB_CB_ADAPTIVE_THRESH) ;
if(found)
{
// Subpixel fine-tuning stuff
- cvFindCornerSubPix(cv_image, &cv_corners[0], num_corners,
+ cvFindCornerSubPix(cv_image_scaled, &cv_corners[0], num_corners,
cvSize(2,2),
cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2)) ;
}
+ // Downscale the pixel locations
+ for (unsigned int i=0; i<cv_corners.size(); i++)
+ {
+ cv_corners[i].x /= scale ;
+ cv_corners[i].y /= scale ;
+ }
+
robot_msgs::PointCloud cloud ;
cloud.header.frame_id = msg->header.frame_id ;
cloud.header.stamp = msg->header.stamp ;
@@ -137,6 +148,7 @@
}
img_bridge_.fromIpltoRosImage(cv_debug, debug_image_) ;
+ debug_image_.header.stamp = msg->header.stamp ;
debug_pub_.publish(debug_image_) ;
}
}
Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp 2009-06-09 20:15:55 UTC (rev 16870)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_helper.cpp 2009-06-09 20:19:58 UTC (rev 16871)
@@ -58,6 +58,22 @@
bool left_found = left_helper_.getCorners(left_bridge_.toIpl(), left_corners) ;
bool right_found = right_helper_.getCorners(right_bridge_.toIpl(), right_corners) ;
+ left_xy_.resize(left_corners.size()) ;
+ for (unsigned int i=0; i<left_corners.size(); i++)
+ {
+ left_xy_[i].x = left_corners[i].x ;
+ left_xy_[i].y = left_corners[i].y ;
+ left_xy_[i].z = 0.0 ;
+ }
+
+ right_xy_.resize(right_corners.size()) ;
+ for (unsigned int i=0; i<right_corners.size(); i++)
+ {
+ right_xy_[i].x = right_corners[i].x ;
+ right_xy_[i].y = right_corners[i].y ;
+ right_xy_[i].z = 0.0 ;
+ }
+
// Build Debug Images
IplImage* left_debug = cvCreateImage(cvGetSize(left_bridge_.toIpl()), IPL_DEPTH_8U, 3) ;
IplImage* right_debug = cvCreateImage(cvGetSize(right_bridge_.toIpl()), IPL_DEPTH_8U, 3) ;
@@ -72,6 +88,8 @@
left_bridge_.fromIpltoRosImage(left_debug, left_ros_debug_) ;
right_bridge_.fromIpltoRosImage(right_debug, right_ros_debug_) ;
+ left_ros_debug_.header.stamp = left.header.stamp ;
+ right_ros_debug_.header.stamp = right.header.stamp ;
cvReleaseImage(&left_debug) ;
cvReleaseImage(&right_debug) ;
Modified: pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp
===================================================================
--- pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-06-09 20:15:55 UTC (rev 16870)
+++ pkg/trunk/vision/stereo_checkerboard_detector/src/stereo_checkerboard_node.cpp 2009-06-09 20:19:58 UTC (rev 16871)
@@ -64,6 +64,8 @@
sync_.subscribe("stereo/right/cam_info", right_info_msg_, 1) ;
node_->advertise<robot_msgs::PointCloud>("cb_corners", 1) ;
+ node_->advertise<robot_msgs::PointCloud>("cb_corners_left", 1) ;
+ node_->advertise<robot_msgs::PointCloud>("cb_corners_right", 1) ;
node_->advertise<robot_msgs::PoseStamped>("cb_pose", 1) ;
node_->advertise<image_msgs::Image>("left_cb_debug",1) ;
node_->advertise<image_msgs::Image>("right_cb_debug",1) ;
@@ -77,28 +79,45 @@
}
+ void pointVecToCloud(const vector<robot_msgs::Point>& vec, robot_msgs::PointCloud& cloud)
+ {
+ cloud.pts.resize(vec.size()) ;
+ for (unsigned int i=0; i<vec.size(); i++)
+ {
+ cloud.pts[i].x = vec[i].x ;
+ cloud.pts[i].y = vec[i].y ;
+ cloud.pts[i].z = vec[i].z ;
+ }
+ }
+
void msgCallback(ros::Time t)
{
bool found = cb_helper_.findCheckerboard(left_image_msg_, right_image_msg_, left_info_msg_, right_info_msg_) ;
+ // Define the cloud we'll be using to publish with
+ robot_msgs::PointCloud cloud ;
+ cloud.header.frame_id = left_image_msg_.header.frame_id ;
+ cloud.header.stamp = left_image_msg_.header.stamp ;
if (found)
{
- vector<robot_msgs::Point> xyz ;
- cb_helper_.getCorners(xyz) ;
+ //unsigned int N = xyz.size() ;
- unsigned int N = xyz.size() ;
- robot_msgs::PointCloud cloud ;
- cloud.pts.resize(N) ;
- for(unsigned int i=0; i<N; i++)
- {
- cloud.pts[i].x = xyz[i].x ;
- cloud.pts[i].y = xyz[i].y ;
- cloud.pts[i].z = xyz[i].z ;
- }
- cloud.header.frame_id = left_image_msg_.header.frame_id ;
- cloud.header.stamp = left_image_msg_.header.stamp ;
+ // Publish the XYZ corners
+ vector<robot_msgs::Point> point_vec ;
+ cb_helper_.getCorners(point_vec) ;
+ pointVecToCloud(point_vec, cloud) ;
node_->publish("cb_corners", cloud) ;
+ // Publish the left corners
+ cb_helper_.getCornersLeft(point_vec) ;
+ pointVecToCloud(point_vec, cloud) ;
+ node_->publish("cb_corners_left", cloud) ;
+
+ // Publish the right corners
+ cb_helper_.getCornersRight(point_vec) ;
+ pointVecToCloud(point_vec, cloud) ;
+ node_->publish("cb_corners_right", cloud) ;
+
// Publish a mini-axes defining the checkerboard
tf::Pose pose ;
cb_helper_.getPose(pose) ;
@@ -113,7 +132,12 @@
printf("Found CB!\n") ;
}
else
+ {
+ cloud.pts.clear() ;
+ node_->publish("cb_corners_left", cloud) ;
+ node_->publish("cb_corners_right", cloud) ;
printf("Not found\n") ;
+ }
node_->publish("left_cb_debug", cb_helper_.getLeftDebug()) ;
node_->publish("right_cb_debug", cb_helper_.getRightDebug()) ;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-06-10 04:32:37
|
Revision: 16915
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16915&view=rev
Author: gerkey
Date: 2009-06-10 04:30:08 +0000 (Wed, 10 Jun 2009)
Log Message:
-----------
Commented macro that's only available on ros trunk at the moment.
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-10 04:11:15 UTC (rev 16914)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-10 04:30:08 UTC (rev 16915)
@@ -9,7 +9,7 @@
src/collision_models.cpp
src/kinematic_model_state_monitor.cpp
src/collision_space_monitor.cpp)
-rospack_add_openmp_flags(planning_environment)
+#rospack_add_openmp_flags(planning_environment)
# Tests
Modified: pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-10 04:11:15 UTC (rev 16914)
+++ pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-10 04:30:08 UTC (rev 16915)
@@ -28,4 +28,4 @@
#rospack_link_boost(${PROJECT_NAME} thread)
rospack_add_executable(test_filter test_filter.cpp)
-rospack_add_openmp_flags(test_filter)
+#rospack_add_openmp_flags(test_filter)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-06-10 23:36:50
|
Revision: 16953
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16953&view=rev
Author: hsujohnhsu
Date: 2009-06-10 23:36:45 +0000 (Wed, 10 Jun 2009)
Log Message:
-----------
fix erratic demo.
Modified Paths:
--------------
pkg/trunk/demos/erratic_gazebo/erratic.launch
pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world
Modified: pkg/trunk/demos/erratic_gazebo/erratic.launch
===================================================================
--- pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-06-10 23:29:11 UTC (rev 16952)
+++ pkg/trunk/demos/erratic_gazebo/erratic.launch 2009-06-10 23:36:45 UTC (rev 16953)
@@ -10,7 +10,7 @@
<param name="robotdesc/erratic" command="$(find xacro)/xacro.py '$(find erratic_defs)/robots/erratic.xml'" />
<!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple_erratic.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
Modified: pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml
===================================================================
--- pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-06-10 23:29:11 UTC (rev 16952)
+++ pkg/trunk/robot_descriptions/erratic/erratic_defs/robots/erratic.xml 2009-06-10 23:36:45 UTC (rev 16953)
@@ -66,7 +66,7 @@
<collision>
<origin xyz="-0.075 0 0.115" rpy="0 0 0" />
- <geometry name="box">
+ <geometry name="base_collision">
<box size="0.22 0.29 0.14" />
</geometry>
</collision>
@@ -79,7 +79,7 @@
<joint name="base_top_joint" type="fixed" />
<inertial>
- <mass value="0.01"/>
+ <mass value="1.0"/>
<com xyz="0 0 ${base_size_z/2}" />
<inertia ixx="0.0652232699207" ixy="0.0" ixz="0.0"
iyy="0.0669473158652" iyz="0.0"
@@ -91,7 +91,7 @@
<map name="gazebo_material" flag="gazebo">
<elem key="material" value="Gazebo/Blue" />
</map>
- <geometry name="base_visual">
+ <geometry name="base_top_visual">
<mesh scale="${base_size_x} ${base_size_y} 0.01" />
</geometry>
</visual>
@@ -99,7 +99,7 @@
<collision>
<origin xyz="-0.08 0 0.13" rpy="0 0 0" />
- <geometry name="box">
+ <geometry name="base_top_collision">
<box size="${base_size_x} ${base_size_y} 0.01" />
</geometry>
</collision>
@@ -120,7 +120,7 @@
<joint name="${parent}_${suffix}_wheel_joint" />
<origin xyz="0 ${reflect*caster_wheel_offset_y} ${wheel_radius}" rpy="0 0 0" />
<inertial>
- <mass value="0.01" /> <!-- check jmh 20081205 -->
+ <mass value="0.1" /> <!-- check jmh 20081205 -->
<com xyz=" 0 0 0 " />
<inertia ixx="0.012411765597" ixy="0.0" ixz="0.0"
iyy="0.015218160428" iyz="0.0"
@@ -174,14 +174,14 @@
<map name="gazebo_material" flag="gazebo">
<elem key="material" value="Gazebo/Black" />
</map>
- <geometry name="base_visual">
+ <geometry name="base_caster_box_visual">
<mesh scale="0.09 0.02 0.10" />
</geometry>
</visual>
<collision>
<origin xyz="-0.23 0 0.08" rpy="0 0 0" />
- <geometry name="box">
+ <geometry name="box_caster_box_collision">
<box size="0.09 0.02 0.10" />
</geometry>
</collision>
@@ -202,7 +202,7 @@
<joint name="base_caster_support_joint" type="fixed"/>
-->
<inertial>
- <mass value="0.01"/>
+ <mass value="0.1"/>
<com xyz="0 0 0" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
@@ -214,14 +214,14 @@
<map name="gazebo_material" flag="gazebo">
<elem key="material" value="Gazebo/Grey" />
</map>
- <geometry name="base_visual">
+ <geometry name="base_caster_support_visual">
<mesh scale="0.03 0.02 0.06" />
</geometry>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
- <geometry name="box">
+ <geometry name="base_caster_support_collision">
<box size="0.03 0.02 0.06" />
</geometry>
</collision>
@@ -249,7 +249,7 @@
<joint name="caster_wheel_joint" />
<origin xyz="-0.02 0 -0.02" rpy="0 0 0" />
<inertial>
- <mass value="0.01" />
+ <mass value="0.1" />
<com xyz=" 0 0 0 " />
<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943" />
@@ -304,7 +304,7 @@
<origin rpy="0 0 0" xyz="0.0 0 0.205"/>
<joint name="base_laser_joint" type="fixed"/>
<inertial>
- <mass value="0.01"/>
+ <mass value="0.1"/>
<com xyz="0 0 0.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
@@ -329,6 +329,7 @@
<map flag="gazebo" name="sensor">
<verbatim key="sensor_base_laser_ray">
<sensor:ray name="base_laser">
+ <resRange>0.1</resRange>
<rayCount>640</rayCount>
<rangeCount>640</rangeCount>
<laserCount>1</laserCount>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world 2009-06-10 23:29:11 UTC (rev 16952)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/simple_erratic.world 2009-06-10 23:36:45 UTC (rev 16953)
@@ -27,9 +27,9 @@
<stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.000000000001</cfm>
- <erp>0.2</erp>
+ <erp>0.5</erp>
<quickStep>true</quickStep>
- <quickStepIters>300</quickStepIters>
+ <quickStepIters>100</quickStepIters>
<quickStepW>1.3</quickStepW>
</physics:ode>
@@ -39,7 +39,7 @@
<rendering:gui>
<type>fltk</type>
- <imageSize>1024 800</imageSize>
+ <size>1024 800</size>
<pos>0 0</pos>
<frames>
<row height="100%">
@@ -70,6 +70,7 @@
<body:plane name="plane">
<geom:plane name="plane">
+ <laserRetro>2000.0</laserRetro>
<kp>1000000.0</kp>
<kd>1.0</kd>
<normal>0 0 1</normal>
@@ -82,7 +83,7 @@
<!-- The "desk"-->
<!-- small desks -->
- <model:physical name="../gazebo_objects/desk1_model">
+ <model:physical name="desk1_model">
<xyz>2.28 -0.21 0</xyz>
<rpy>0.0 0.0 0.0 </rpy>
<include embedded="true">
@@ -92,7 +93,7 @@
</model:physical>
<!-- The second "desk"-->
- <model:physical name="../gazebo_objects/desk2_model">
+ <model:physical name="desk2_model">
<xyz>2.25 -3.0 0</xyz>
<rpy>0.0 0.0 0.0 </rpy>
<include embedded="true">
@@ -116,6 +117,7 @@
<material>Gazebo/PioneerBody</material>
<mesh>unit_cylinder</mesh>
</visual>
+ <laserRetro>1.0</laserRetro>
</geom:cylinder>
<geom:box name="cylinder1_base_geom">
<kp>100000000.0</kp>
@@ -129,12 +131,13 @@
<material>Gazebo/Fish</material>
<mesh>unit_box</mesh>
</visual>
+ <laserRetro>1.0</laserRetro>
</geom:box>
</body:cylinder>
</model:physical>
<!-- The small box "cup" -->
- <model:physical name="../gazebo_objects/object1">
+ <model:physical name="object1">
<xyz>2.28 -0.21 0</xyz>
<rpy>0.0 0.0 0.0 </rpy>
<include embedded="true">
@@ -239,6 +242,7 @@
</model:physical>
<model:empty name="factory_model">
+ <model_thread>false</model_thread>
<controller:factory name="factory_controller">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
@@ -247,7 +251,6 @@
</model:empty>
<!-- White Directional light -->
- <!--
<model:renderable name="directional_white">
<light>
<type>directional</type>
@@ -255,8 +258,10 @@
<diffuseColor>0.4 0.4 0.4</diffuseColor>
<specularColor>0.0 0.0 0.0</specularColor>
<attenuation>1 0.0 1.0 0.4</attenuation>
+ <range>10</range>
</light>
</model:renderable>
+ <!--
-->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-06-12 02:23:03
|
Revision: 17018
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17018&view=rev
Author: eitanme
Date: 2009-06-12 02:22:59 +0000 (Fri, 12 Jun 2009)
Log Message:
-----------
Added support for BaseGlobalPlanner and BaseLocalPlanner interfaces. Includes a factory for each that makes use of different global/local planners with MoveBase easy.
Modified Paths:
--------------
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/manifest.xml
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/nav_robot_actions/manifest.xml
Added Paths:
-----------
pkg/trunk/nav/nav_robot_actions/include/
pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/
pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h
pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h
Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-12 02:22:59 UTC (rev 17018)
@@ -39,17 +39,18 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
#include <nav_robot_actions/MoveBaseState.h>
+#include <nav_robot_actions/base_local_planner.h>
+#include <nav_robot_actions/base_global_planner.h>
#include <robot_msgs/PoseStamped.h>
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/rate.h>
-#include <navfn/navfn_ros.h>
-#include <base_local_planner/trajectory_planner_ros.h>
#include <vector>
#include <string>
#include <nav_srvs/Plan.h>
#include <visualization_msgs/Marker.h>
+#include <robot_msgs/PoseDot.h>
namespace move_base {
/**
@@ -62,9 +63,10 @@
* @brief Constructor for the actions
* @param ros_node A reference to the ros node used
* @param tf A reference to a TransformListener
- * @return
+ * @param global_planner The name of the global planner to use
+ * @param local_planner The name of the local planner to use
*/
- MoveBase(ros::Node& ros_node, tf::TransformListener& tf);
+ MoveBase(ros::Node& ros_node, tf::TransformListener& tf, std::string global_planner, std::string local_planner);
/**
* @brief Destructor - Cleans up
@@ -120,11 +122,10 @@
ros::Node& ros_node_;
tf::TransformListener& tf_;
- bool run_planner_;
- base_local_planner::TrajectoryPlannerROS* tc_;
+ nav_robot_actions::BaseLocalPlanner* tc_;
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
- navfn::NavfnROS* planner_;
+ nav_robot_actions::BaseGlobalPlanner* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
std::vector<robot_msgs::Point> footprint_;
std::string robot_base_frame_, global_frame_;
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-12 02:22:59 UTC (rev 17018)
@@ -38,9 +38,7 @@
#include <cstdlib>
#include <ctime>
-using namespace base_local_planner;
using namespace costmap_2d;
-using namespace navfn;
using namespace robot_actions;
namespace move_base {
@@ -49,9 +47,9 @@
return x < 0.0 ? -1.0 : 1.0;
}
- MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) :
+ MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf, std::string global_planner, std::string local_planner) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
- run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
+ tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
planner_(NULL), valid_plan_(false), new_plan_(false), attempted_rotation_(false), attempted_costmap_reset_(false),
done_half_rotation_(false), done_full_rotation_(false), escaping_(false) {
@@ -125,15 +123,27 @@
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"), footprint_);
- //initialize the NavFn planner
- planner_ = new NavfnROS(ros_node_, tf_, *planner_costmap_ros_);
+ //initialize the global planner
+ try {
+ planner_ = nav_robot_actions::BGPFactory::Instance().CreateObject(global_planner, ros_node_, tf_, *planner_costmap_ros_);
+ } catch (Loki::DefaultFactoryError<std::string, nav_robot_actions::BaseGlobalPlanner>::Exception)
+ {
+ ROS_ASSERT_MSG(false, "Cannot create the desired global planner because it is not registered with the factory");
+ }
+
ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->cellSizeX(), planner_costmap_ros_->cellSizeY());
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"), footprint_);
- //create a trajectory controller
- tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
+ //create a local planner
+ try {
+ tc_ = nav_robot_actions::BLPFactory::Instance().CreateObject(local_planner,
+ ros_node_, tf_, *controller_costmap_ros_, footprint_);
+ } catch (Loki::DefaultFactoryError<std::string, nav_robot_actions::BaseLocalPlanner>::Exception)
+ {
+ ROS_ASSERT_MSG(false, "Cannot create the desired local planner because it is not registered with the factory");
+ }
//advertise a service for getting a plan
ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -579,7 +589,7 @@
ros::Node ros_node("move_base");
tf::TransformListener tf(ros_node, true, ros::Duration(10));
- move_base::MoveBase move_base(ros_node, tf);
+ move_base::MoveBase move_base(ros_node, tf, "NavfnROS", "TrajectoryPlannerROS");
robot_actions::ActionRunner runner(20.0);
runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-12 02:22:59 UTC (rev 17018)
@@ -194,8 +194,8 @@
bool valid_control = false;
- //compute veloctiy commands to send to the base... don't prune the path
- valid_control = tc_->computeVelocityCommands(cmd_vel, false);
+ //compute veloctiy commands to send to the base
+ valid_control = tc_->computeVelocityCommands(cmd_vel);
//give the base the velocity command
ros_node_.publish("cmd_vel", cmd_vel);
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-06-12 02:22:59 UTC (rev 17018)
@@ -46,9 +46,10 @@
#include <tf/transform_listener.h>
#include <vector>
#include <robot_msgs/Point.h>
+#include <nav_robot_actions/base_global_planner.h>
namespace navfn {
- class NavfnROS {
+ class NavfnROS : public nav_robot_actions::BaseGlobalPlanner {
public:
/**
* @brief Constructor for the NavFnROS object
Modified: pkg/trunk/motion_planning/navfn/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/navfn/manifest.xml 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/manifest.xml 2009-06-12 02:22:59 UTC (rev 17018)
@@ -9,6 +9,7 @@
<depend package="roscpp"/>
<depend package="robot_msgs"/>
<depend package="costmap_2d"/>
+ <depend package="nav_robot_actions"/>
<depend package="tf"/>
<depend package="visualization_msgs"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libfltk1.1-dev"/> <!-- actually optional -->
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-06-12 02:22:59 UTC (rev 17018)
@@ -37,7 +37,11 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros) : ros_node_(ros_node), tf_(tf),
+ //register this factory with the ros BaseGlobalPlanner factory
+ ROS_REGISTER_BGP(NavfnROS);
+
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros)
+ : ros_node_(ros_node), tf_(tf),
costmap_ros_(costmap_ros), planner_(costmap_ros.cellSizeX(), costmap_ros.cellSizeY()) {
//get an initial copy of the costmap
costmap_ros_.getCostmapCopy(costmap_);
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-06-12 02:22:59 UTC (rev 17018)
@@ -67,12 +67,14 @@
#include <angles/angles.h>
+#include <nav_robot_actions/base_local_planner.h>
+
namespace base_local_planner {
/**
* @class TrajectoryPlannerROS
* @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
*/
- class TrajectoryPlannerROS{
+ class TrajectoryPlannerROS : public nav_robot_actions::BaseLocalPlanner {
public:
/**
* @brief Constructs the ros wrapper
@@ -101,11 +103,9 @@
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
- * @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
- * @param prune_plan Set to true if you would like the plan to be pruned as the robot drives, false to leave the plan as is
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan = true);
+ bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel);
/**
* @brief Update the plan that the controller is following
@@ -215,6 +215,7 @@
costmap_2d::Costmap2DPublisher* costmap_publisher_;
std::vector<robot_msgs::PoseStamped> global_plan_;
double transform_tolerance_, update_plan_tolerance_;
+ bool prune_plan_;
};
};
Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/manifest.xml 2009-06-12 02:22:59 UTC (rev 17018)
@@ -18,6 +18,7 @@
<depend package="voxel_grid" />
<depend package="angles" />
<depend package="visualization_msgs"/>
+ <depend package="nav_robot_actions"/>
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-12 02:22:59 UTC (rev 17018)
@@ -336,7 +336,7 @@
if(!heading_scoring_)
cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
else
- cost = occdist_scale_ * occ_cost + heading_diff + goal_dist * gdist_scale_;
+ cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
traj.cost_ = cost;
}
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-06-12 02:22:59 UTC (rev 17018)
@@ -47,6 +47,9 @@
using namespace laser_scan;
namespace base_local_planner {
+ //register this base local planner with the BaseLocalPlanner factory
+ ROS_REGISTER_BLP(TrajectoryPlannerROS);
+
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
Costmap2DROS& costmap_ros, std::vector<Point> footprint_spec)
: world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf), ros_node_(ros_node),
@@ -71,6 +74,7 @@
ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
ros_node.param("~base_local_planner/transform_tolerance", transform_tolerance_, 0.2);
ros_node.param("~base_local_planner/update_plan_tolerance", update_plan_tolerance_, 1.0);
+ ros_node.param("~base_local_planner/prune_plan", prune_plan_, true);
double map_publish_frequency;
ros_node.param("~base_local_planner/costmap_visualization_rate", map_publish_frequency, 2.0);
@@ -294,7 +298,7 @@
return true;
}
- bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan){
+ bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel){
//assume at the beginning of our control cycle that we could have a new goal
goal_reached_ = false;
@@ -336,7 +340,7 @@
}
//now we'll prune the plan based on the position of the robot
- if(prune_plan)
+ if(prune_plan_)
prunePlan(global_pose, transformed_plan, global_plan_);
Added: pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h
===================================================================
--- pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h 2009-06-12 02:22:59 UTC (rev 17018)
@@ -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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_ROBOT_ACTIONS_BASE_GLOBAL_PLANNER_
+#define NAV_ROBOT_ACTIONS_BASE_GLOBAL_PLANNER_
+
+#include <robot_msgs/PoseStamped.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <loki/Factory.h>
+#include <loki/Sequence.h>
+
+namespace nav_robot_actions {
+ class BaseGlobalPlanner{
+ public:
+ virtual bool makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan) = 0;
+
+ protected:
+ BaseGlobalPlanner(){}
+ };
+
+ //create an associated factory
+ typedef Loki::SingletonHolder
+ <
+ Loki::Factory< BaseGlobalPlanner, std::string,
+ Loki::Seq< ros::Node&,
+ tf::TransformListener&,
+ costmap_2d::Costmap2DROS& > >,
+ Loki::CreateUsingNew,
+ Loki::LongevityLifetime::DieAsSmallObjectParent
+ > BGPFactory;
+
+#define ROS_REGISTER_BGP(c) \
+ nav_robot_actions::BaseGlobalPlanner* ROS_New_##c(ros::Node& ros_node, tf::TransformListener& tf, \
+ costmap_2d::Costmap2DROS& costmap_ros){ \
+ return new c(ros_node, tf, costmap_ros); \
+ } \
+ class RosBGP##c { \
+ public: \
+ RosBGP##c() \
+ { \
+ nav_robot_actions::BGPFactory::Instance().Register(#c, ROS_New_##c); \
+ } \
+ ~RosBGP##c() \
+ { \
+ nav_robot_actions::BGPFactory::Instance().Unregister(#c); \
+ } \
+ }; \
+ static RosBGP##c ROS_BGP_##c;
+};
+
+#endif
Added: pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h
===================================================================
--- pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h 2009-06-12 02:22:59 UTC (rev 17018)
@@ -0,0 +1,88 @@
+/*********************************************************************
+*
+* 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 Willow Garage, Inc. 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_ROBOT_ACTIONS_BASE_LOCAL_PLANNER_
+#define NAV_ROBOT_ACTIONS_BASE_LOCAL_PLANNER_
+
+#include <robot_msgs/PoseStamped.h>
+#include <robot_msgs/PoseDot.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <loki/Factory.h>
+#include <loki/Sequence.h>
+
+namespace nav_robot_actions {
+ class BaseLocalPlanner{
+ public:
+ virtual bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel) = 0;
+ virtual bool goalReached() = 0;
+ virtual bool updatePlan(const std::vector<robot_msgs::PoseStamped>& plan) = 0;
+
+ protected:
+ BaseLocalPlanner(){}
+ };
+
+ //create an associated factory
+ typedef Loki::SingletonHolder
+ <
+ Loki::Factory< BaseLocalPlanner, std::string,
+ Loki::Seq< ros::Node&,
+ tf::TransformListener&,
+ costmap_2d::Costmap2DROS&,
+ std::vector<robot_msgs::Point> > >,
+ Loki::CreateUsingNew,
+ Loki::LongevityLifetime::DieAsSmallObjectParent
+ > BLPFactory;
+
+#define ROS_REGISTER_BLP(c) \
+ nav_robot_actions::BaseLocalPlanner* ROS_New_##c(ros::Node& ros_node, tf::TransformListener& tf, \
+ costmap_2d::Costmap2DROS& costmap_ros, std::vector<robot_msgs::Point> footprint){ \
+ return new c(ros_node, tf, costmap_ros, footprint); \
+ } \
+ class RosBLP##c { \
+ public: \
+ RosBLP##c() \
+ { \
+ nav_robot_actions::BLPFactory::Instance().Register(#c, ROS_New_##c); \
+ } \
+ ~RosBLP##c() \
+ { \
+ nav_robot_actions::BLPFactory::Instance().Unregister(#c); \
+ } \
+ }; \
+ static RosBLP##c ROS_BLP_##c;
+};
+
+#endif
Modified: pkg/trunk/nav/nav_robot_actions/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_robot_actions/manifest.xml 2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/nav_robot_actions/manifest.xml 2009-06-12 02:22:59 UTC (rev 17018)
@@ -1,6 +1,6 @@
<package>
- <description brief="This package contains core interfaces for robot actions"/>
- <author>Conor McGann</author>
+ <description brief="This package contains core interfaces for navigation robot actions"/>
+ <author>Eitan Marder-Eppstein</author>
<license>BSD</license>
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com</url>
@@ -8,10 +8,11 @@
<depend package="robot_actions"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
+ <depend package="loki"/>
<export>
- <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_actions"/>
+ <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-13 02:22:13
|
Revision: 17056
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17056&view=rev
Author: isucan
Date: 2009-06-13 01:37:29 +0000 (Sat, 13 Jun 2009)
Log Message:
-----------
simplifying PoseConstraint message and getting robot pose from TF rather than a localized pose
Modified Paths:
--------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/util/self_watch/self_watch.cpp
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-06-13 01:37:29 UTC (rev 17056)
@@ -2,8 +2,6 @@
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints
-Header header
-
# Constants that represent possible values for type. A position and an
# orientation constant can be combined (by adding).
@@ -31,16 +29,9 @@
# The robot link this constraint refers to
string link_name
-# The desired pose of the robot link (in the robot frame)
-float64 x
-float64 y
-float64 z
+# The desired pose of the robot link
+robot_msgs/PoseStamped pose
-# euler angles around YXZ
-float64 roll # around Z axis
-float64 pitch # around X axis
-float64 yaw # around Y axis
-
# the allowed difference (square of euclidian distance) for position
float64 position_distance
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -98,7 +98,7 @@
id_ = 0;
visualizationMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, false);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1));
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -57,7 +57,7 @@
{
// register with ROS
m_collisionModels = new planning_environment::CollisionModels("robot_description");
- m_collisionSpaceMonitor = new planning_environment::CollisionSpaceMonitor(m_collisionModels, false);
+ m_collisionSpaceMonitor = new planning_environment::CollisionSpaceMonitor(m_collisionModels);
m_collisionSpaceMonitor->setOnAfterMapUpdateCallback(boost::bind(&KinematicPlanning::afterWorldUpdate, this, _1));
// status info
@@ -120,16 +120,13 @@
if (execute)
{
- ROS_INFO("Working in frame %s", m_collisionSpaceMonitor->getFrameId().c_str());
+ ROS_INFO("Motion planning running in frame '%s'", m_collisionSpaceMonitor->getFrameId().c_str());
startPublishingStatus();
}
}
if (execute)
- {
- ROS_INFO("Motion planning is now available.");
ros::spin();
- }
else
if (mlist.empty())
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
@@ -150,14 +147,6 @@
ROS_WARN("Planning is not safe: kinematic state is not up to date");
return false;
}
- else
- if (m_collisionSpaceMonitor->isPoseIncluded())
- if (!m_collisionSpaceMonitor->isPoseUpdated(m_intervalKinematicState))
- {
- if (report)
- ROS_WARN("Planning is not safe: kinematic state is not up to date");
- return false;
- }
return true;
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -96,7 +96,7 @@
{
stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, false);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&StateValidityMonitor::afterWorldUpdate, this, _1));
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -70,7 +70,7 @@
goalA_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
@@ -146,13 +146,16 @@
req.goal_constraints.pose.resize(1);
req.goal_constraints.pose[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + motion_planning_msgs::PoseConstraint::ORIENTATION_RP;
req.goal_constraints.pose[0].link_name = "r_gripper_r_finger_tip_link";
- req.goal_constraints.pose[0].x = 0.75025;
- req.goal_constraints.pose[0].y = -0.188;
- req.goal_constraints.pose[0].z = 0.829675;
+ req.goal_constraints.pose[0].pose.header.stamp = ros::Time::now();
+ req.goal_constraints.pose[0].pose.header.frame_id = "base_link";
+ req.goal_constraints.pose[0].pose.pose.position.x = 0.75025;
+ req.goal_constraints.pose[0].pose.pose.position.y = -0.188;
+ req.goal_constraints.pose[0].pose.pose.position.z = 0.829675;
- req.goal_constraints.pose[0].roll = 0.0;
- req.goal_constraints.pose[0].pitch = 0.0;
- req.goal_constraints.pose[0].yaw = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.x = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.y = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.z = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.w = 1.0;
req.goal_constraints.pose[0].position_distance = 0.0001;
req.goal_constraints.pose[0].orientation_distance = 0.1;
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -63,7 +63,7 @@
plan_id_ = -1;
robot_stopped_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -52,7 +52,7 @@
TestExecutionPath(void)
{
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
sleep_duration_ = 4;
use_topic_ = false;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -40,7 +40,6 @@
#include "planning_environment/collision_models.h"
#include "planning_environment/kinematic_model_state_monitor.h"
-#include <tf/transform_listener.h>
#include <robot_msgs/CollisionMap.h>
#include <robot_msgs/AttachedObject.h>
@@ -62,13 +61,18 @@
{
public:
- CollisionSpaceMonitor(CollisionModels *cm, bool includePose) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), includePose),
- tf_(*ros::Node::instance())
+ CollisionSpaceMonitor(CollisionModels *cm, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), frame_id)
{
cm_ = cm;
setupCSM();
}
+ CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm))
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
virtual ~CollisionSpaceMonitor(void)
{
}
@@ -123,7 +127,6 @@
void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
- tf::TransformListener tf_;
CollisionModels *cm_;
collision_space::EnvironmentModel *collisionSpace_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -39,8 +39,8 @@
#include "planning_environment/robot_models.h"
#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
#include <robot_msgs/MechanismState.h>
-#include <robot_msgs/PoseWithCovariance.h>
#include <boost/bind.hpp>
#include <vector>
#include <string>
@@ -64,17 +64,27 @@
{
public:
- KinematicModelStateMonitor(RobotModels *rm, bool includePose)
+ KinematicModelStateMonitor(RobotModels *rm)
{
rm_ = rm;
- includePose_ = includePose;
+ includePose_ = false;
setupRSM();
}
+ KinematicModelStateMonitor(RobotModels *rm, const std::string &frame_id)
+ {
+ rm_ = rm;
+ includePose_ = true;
+ frame_id_ = frame_id;
+ setupRSM();
+ }
+
virtual ~KinematicModelStateMonitor(void)
{
if (robotState_)
delete robotState_;
+ if (tf_)
+ delete tf_;
}
/** \brief Define a callback for when the state is changed */
@@ -102,41 +112,32 @@
}
/** \brief Return the frame id of the state */
- const std::string& getFrameId(void) const;
+ const std::string& getFrameId(void) const
+ {
+ return frame_id_;
+ }
- /** \brief Return true if a pose has been received */
- bool havePose(void) const
+ /** \brief Return the robot frame id (robot without pose) */
+ const std::string& getRobotFrameId(void) const
{
- return havePose_;
+ return robot_frame_;
}
-
+
/** \brief Return true if a full mechanism state has been received */
bool haveState(void) const
{
- return haveMechanismState_;
+ return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
}
- /** \brief Return the time of the last pose update */
- const ros::Time& lastPoseUpdate(void) const
- {
- return lastPoseUpdate_;
- }
-
/** \brief Return the time of the last state update */
const ros::Time& lastStateUpdate(void) const
{
return lastStateUpdate_;
}
-
- /** \brief Wait until a pose is received */
- void waitForPose(void) const;
/** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** \brief Return true if a pose has been received in the last sec seconds */
- bool isPoseUpdated(double sec) const;
-
/** \brief Return true if a full mechanism state has been received in the last sec seconds */
bool isStateUpdated(double sec) const;
@@ -152,7 +153,6 @@
protected:
void setupRSM(void);
- void localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose);
void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
@@ -164,10 +164,11 @@
ros::NodeHandle nh_;
ros::Subscriber mechanismStateSubscriber_;
- ros::Subscriber localizedPoseSubscriber_;
+ tf::TransformListener *tf_;
planning_models::KinematicModel::StateParams *robotState_;
tf::Pose pose_;
+ std::string robot_frame_;
std::string frame_id_;
boost::function<void(void)> onStateUpdate_;
@@ -175,7 +176,6 @@
bool havePose_;
bool haveMechanismState_;
ros::Time lastStateUpdate_;
- ros::Time lastPoseUpdate_;
};
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -59,12 +59,16 @@
{
}
+ /** Clear the stored constraint */
virtual void clear(void) = 0;
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
/** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const = 0;
+ /** Print the constraint data */
virtual void print(std::ostream &out = std::cout) const
{
}
@@ -80,13 +84,24 @@
m_kmodel = NULL;
}
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::JointConstraint &jc);
+
+ /** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const;
+
+ /** Clear the stored constraint */
virtual void clear(void);
- const motion_planning_msgs::JointConstraint& getConstraintMessage(void) const;
+
+ /** Print the constraint data */
virtual void print(std::ostream &out = std::cout) const;
+ /** Get the constraint message */
+ const motion_planning_msgs::JointConstraint& getConstraintMessage(void) const;
+
protected:
motion_planning_msgs::JointConstraint m_jc;
@@ -105,19 +120,39 @@
m_link = NULL;
}
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc);
+
+ /** Clear the stored constraint */
virtual void clear(void);
+
+ /** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, int groupID) const;
+
+ /** Evaluate the distances to the position and to the orientation are given. */
void evaluate(double *distPos, double *distAng) const;
+
+ /** Decide whether the constraint is satisfied. The distances to the position and to the orientation are given. */
bool decide(double dPos, double dAng) const;
+ /** Print the constraint data */
+ void print(std::ostream &out = std::cout) const;
+
+ /** Get the constraint message */
const motion_planning_msgs::PoseConstraint& getConstraintMessage(void) const;
- void print(std::ostream &out = std::cout) const;
protected:
motion_planning_msgs::PoseConstraint m_pc;
+ double m_x;
+ double m_y;
+ double m_z;
+ double m_yaw;
+ double m_pitch;
+ double m_roll;
planning_models::KinematicModel::Link *m_link;
};
@@ -136,10 +171,19 @@
clear();
}
+ /** Clear the stored constraints */
void clear(void);
+
+ /** Add a set of joint constraints */
bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::JointConstraint> &jc);
+
+ /** Add a set of pose constraints */
bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc);
+
+ /** Decide whether the set of constraints is satisfied */
bool decide(const double *params, int groupID) const;
+
+ /** Print the constraint data */
void print(std::ostream &out = std::cout) const;
protected:
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -85,7 +85,7 @@
loaded_models_ = false;
loadRobot();
}
-
+
virtual ~RobotModels(void)
{
}
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -52,6 +52,10 @@
onAfterMapUpdate_ = NULL;
onAfterAttachBody_ = NULL;
haveMap_ = false;
+
+ if (!tf_)
+ tf_ = new tf::TransformListener(*ros::Node::instance());
+
collisionSpace_ = cm_->getODECollisionModel().get();
collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
@@ -104,7 +108,7 @@
robot_msgs::PointStamped pso;
try
{
- tf_.transformPoint(target, psi, pso);
+ tf_->transformPoint(target, psi, pso);
}
catch(...)
{
@@ -120,7 +124,7 @@
}
if (err)
- ROS_ERROR("Some errors encountered in transforming the collision map to frame %s from frame %s", target.c_str(), collisionMap->header.frame_id.c_str());
+ ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collisionMap->header.frame_id.c_str());
}
else
{
@@ -177,12 +181,12 @@
bool err = false;
try
{
- tf_.transformPoint(attachedObject->link_name, center, centerP);
+ tf_->transformPoint(attachedObject->link_name, center, centerP);
}
catch(...)
{
err = true;
- ROS_ERROR("Unable to transform object to be attached from frame %s to frame %s", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
+ ROS_ERROR("Unable to transform object to be attached from frame '%s' to frame '%s'", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
}
if (err)
continue;
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -43,6 +43,7 @@
kmodel_ = NULL;
robotState_ = NULL;
onStateUpdate_ = NULL;
+ tf_ = NULL;
havePose_ = haveMechanismState_ = false;
if (rm_->loadedModels())
{
@@ -50,7 +51,10 @@
robotState_ = kmodel_->newStateParams();
if (kmodel_->getRobotCount() > 1)
+ {
ROS_WARN("Using more than one robot. A frame_id cannot be set (there multiple frames) and pose cannot be maintained");
+ includePose_ = false;
+ }
else
{
// joints to update based on received pose
@@ -59,34 +63,23 @@
if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(kmodel_->getRobot(0)->chain))
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
+ robot_frame_ = kmodel_->getRobot(0)->chain->after->name;
+ ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
+
if (includePose_)
{
- localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
- ROS_DEBUG("Listening to localized pose");
+ tf_ = new tf::TransformListener(*ros::Node::instance());
+ ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
}
else
- {
- frame_id_ = kmodel_->getRobot(0)->chain->after->name;
- ROS_DEBUG("Robot state frame is %s", frame_id_.c_str());
- }
+ frame_id_ = robot_frame_;
}
+
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
ROS_DEBUG("Listening to mechanism state");
}
}
-const std::string& planning_environment::KinematicModelStateMonitor::getFrameId(void) const
-{
- if (frame_id_.empty())
- {
- if (includePose_)
- waitForPose();
- if (frame_id_.empty())
- ROS_ERROR("Cannot get frame ID for robot state");
- }
- return frame_id_;
-}
-
void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = !haveMechanismState_;
@@ -124,77 +117,67 @@
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
- if (change && onStateUpdate_ != NULL)
- onStateUpdate_();
-}
-
-void planning_environment::KinematicModelStateMonitor::localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose)
-{
- tf::PoseMsgToTF(localizedPose->pose, pose_);
- lastPoseUpdate_ = localizedPose->header.stamp;
- frame_id_ = localizedPose->header.frame_id;
-
- bool change = !havePose_;
- havePose_ = true;
-
- if (!planarJoint_.empty())
+ if (includePose_)
{
- double planar_joint[3];
- planar_joint[0] = pose_.getOrigin().x();
- planar_joint[1] = pose_.getOrigin().y();
-
- double yaw, pitch, roll;
- pose_.getBasis().getEulerZYX(yaw, pitch, roll);
- planar_joint[2] = yaw;
-
- bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
- change = change || this_changed;
+ // use tf to figure out pose
+ if (tf_->canTransform(frame_id_, robot_frame_, mechanismState->header.stamp))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_->lookupTransform(frame_id_, robot_frame_, mechanismState->header.stamp, transf);
+ pose_ = transf;
+
+ if (!planarJoint_.empty())
+ {
+ double planar_joint[3];
+ planar_joint[0] = pose_.getOrigin().x();
+ planar_joint[1] = pose_.getOrigin().y();
+
+ double yaw, pitch, roll;
+ pose_.getBasis().getEulerZYX(yaw, pitch, roll);
+ planar_joint[2] = yaw;
+
+ bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
+ change = change || this_changed;
+ }
+
+ if (!floatingJoint_.empty())
+ {
+ double floating_joint[7];
+ floating_joint[0] = pose_.getOrigin().x();
+ floating_joint[1] = pose_.getOrigin().y();
+ floating_joint[2] = pose_.getOrigin().z();
+ btQuaternion q = pose_.getRotation();
+ floating_joint[3] = q.x();
+ floating_joint[4] = q.y();
+ floating_joint[5] = q.z();
+ floating_joint[6] = q.w();
+
+ bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
+ change = change || this_changed;
+ }
+
+ havePose_ = true;
+ }
+ else
+ ROS_WARN("Unable fo find transform from link '%s' to link '%s'", robot_frame_.c_str(), frame_id_.c_str());
}
- if (!floatingJoint_.empty())
- {
- double floating_joint[7];
- floating_joint[0] = pose_.getOrigin().x();
- floating_joint[1] = pose_.getOrigin().y();
- floating_joint[2] = pose_.getOrigin().z();
- btQuaternion q = pose_.getRotation();
- floating_joint[3] = q.x();
- floating_joint[4] = q.y();
- floating_joint[5] = q.z();
- floating_joint[6] = q.w();
-
- bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
- change = change || this_changed;
- }
-
if (change && onStateUpdate_ != NULL)
onStateUpdate_();
}
void planning_environment::KinematicModelStateMonitor::waitForState(void) const
{
- while (nh_.ok() && !haveMechanismState_)
+ while (nh_.ok() && !haveState())
{
ROS_INFO("Waiting for mechanism state ...");
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
- if (haveMechanismState_)
+ if (haveState())
ROS_INFO("Mechanism state received!");
}
-void planning_environment::KinematicModelStateMonitor::waitForPose(void) const
-{
- while (nh_.ok() && !havePose_)
- {
- ROS_INFO("Waiting for robot pose ...");
- ros::spinOnce();
- ros::Duration().fromSec(0.05).sleep();
- }
- if (havePose_)
- ROS_INFO("Robot pose received!");
-}
-
bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
{
if (sec > 0 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
@@ -203,14 +186,6 @@
return true;
}
-bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
-{
- if (sec > 0 && lastPoseUpdate_ < ros::Time::now() - ros::Duration(sec))
- return false;
- else
- return true;
-}
-
void planning_environment::KinematicModelStateMonitor::printRobotState(void) const
{
std::stringstream ss;
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -35,6 +35,7 @@
/** \author Ioan Sucan */
#include "planning_environment/kinematic_state_constraint_evaluator.h"
+#include <tf/transform_datatypes.h>
#include <cassert>
bool planning_environment::JointConstraintEvaluator::use(const planning_models::KinematicModel *kmodel, const ros::Message *kc)
@@ -114,6 +115,14 @@
{
m_link = kmodel->getLink(pc.link_name);
m_pc = pc;...
[truncated message content] |
|
From: <is...@us...> - 2009-06-14 00:59:38
|
Revision: 17064
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17064&view=rev
Author: isucan
Date: 2009-06-14 00:59:35 +0000 (Sun, 14 Jun 2009)
Log Message:
-----------
move_arm can now contain the replanning & path monitoring logic
Modified Paths:
--------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPProjectionEvaluators.h
pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.h
Removed Paths:
-------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/DisplayKinematicPath.msg
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPlanStatus.msg
Deleted: pkg/trunk/motion_planning/motion_planning_msgs/msg/DisplayKinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/DisplayKinematicPath.msg 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/DisplayKinematicPath.msg 2009-06-14 00:59:35 UTC (rev 17064)
@@ -1,6 +0,0 @@
-# The definition of a kinematic path that has a name
-# The name identifies the part of the robot the path is for
-string frame_id
-string model_name
-KinematicState start_state
-KinematicPath path
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/JointConstraint.msg 2009-06-14 00:59:35 UTC (rev 17064)
@@ -1,3 +1,5 @@
+Header header
+
# Constrain the position of a joint to be within a certain bound
string joint_name
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPath.msg 2009-06-14 00:59:35 UTC (rev 17064)
@@ -1,11 +1,13 @@
-# The definition of a kinematic path. the
-# joint names the states correspond to and the times at which
-# each state should be along the path
+# The definition of a kinematic path.
+Header header
# The full state of the robot at the start of the path
# As described by KinematicModel::StateParams
KinematicState start_state
+# The model for which this plan was computed
+string model_id
+
# The joint names, in the same order as the values in the state
string[] names
Deleted: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPlanStatus.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPlanStatus.msg 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicPlanStatus.msg 2009-06-14 00:59:35 UTC (rev 17064)
@@ -1,35 +0,0 @@
-# This message contains the definition for the state of a motion plan
-
-# The ID of the request the status message refers to. A value of -1 means that
-# replanning is not active
-int32 id
-
-# The result of a motion plan: a kinematic path. If the motion plan is
-# not succesful, this path has 0 states. If the motion plan is
-# succesful, this path contains the minimum number of states (but it
-# includes start and goal states) to define the motions for the
-# robot. If more intermediate states are needed, linear interpolation
-# is to be assumed.
-motion_planning_msgs/KinematicPath path
-
-# If the last sent path is valid, this value is set to 1.
-# When this valid is set to 0, the path became invalid and
-# replanning starts.
-byte valid
-
-# The distance to the goal that was actually achieved (heuristic). If the planner did not have enough time,
-# it may not return a complete solution return a distance larger than the desired threshold. The user may
-# choose to use this path anyway
-float64 distance
-
-# If computing the exact path was not possible, but an approximate
-# solution is obtained, this will be 1; otherwise, it will be 0
-byte approximate
-
-# If the data the planner has when planning is stale, set this flag to 1
-# This would happen when map data has not been received recently
-byte unsafe
-
-# If the starting state is already in the goal region,
-# this is set to 1. Otherwise, it will be set to 0
-byte done
Modified: pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv
===================================================================
--- pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/motion_planning_srvs/srv/KinematicPlan.srv 2009-06-14 00:59:35 UTC (rev 17064)
@@ -21,5 +21,14 @@
---
-# A handle for the motion plan. Future status messages will be marked with this ID.
-int32 id
+# A solution path, if found
+motion_planning_msgs/KinematicPath path
+
+# distance to goal as computed by the heuristic
+float64 distance
+
+# set to 1 if the computed path was approximate
+byte approximate
+
+# if state information was not up to date when planning, this is set to 1
+byte unsafe
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/RKPRequestHandler.h 2009-06-14 00:59:35 UTC (rev 17064)
@@ -38,13 +38,8 @@
#define KINEMATIC_PLANNING_RKP_REQUEST_HANDLER_
#include "kinematic_planning/RKPModel.h"
-#include <motion_planning_msgs/KinematicPath.h>
#include <motion_planning_srvs/KinematicPlan.h>
-#include <ros/console.h>
-#include <iostream>
-#include <sstream>
-
/** Main namespace */
namespace kinematic_planning
{
@@ -58,54 +53,28 @@
RKPRequestHandler(void)
{
- m_activePSetup = NULL;
}
~RKPRequestHandler(void)
{
}
- /** Set up all the data needed by motion planning based on a request and lock the planner setup
- * using this data */
- bool configure(ModelMap &models, const motion_planning_msgs::KinematicState &startState, const motion_planning_srvs::KinematicPlan::Request &req);
- void release(void);
+ bool isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req);
+
+ /* Check and compute a motion plan. Return true if the plan was succesfully computed */
+ bool computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res);
- bool isActive(void);
- RKPPlannerSetup *activePlannerSetup(void);
- motion_planning_srvs::KinematicPlan::Request& activeRequest(void);
- motion_planning_msgs::KinematicState& activeStartState(void);
- bool isStillValid(motion_planning_msgs::KinematicPath &path);
- bool isTrivial(double *distance = NULL);
- void execute(motion_planning_msgs::KinematicPath &path, double &distance, bool &trivial, bool &approximate);
-
protected:
- bool isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req);
- bool areSpaceParamsValid(const ModelMap &modelsRef, motion_planning_msgs::KinematicSpaceParameters ¶ms) const;
-
+ /** Set up all the data needed by motion planning based on a request and lock the planner setup
+ * using this data */
+ void configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup);
- void update(void);
- void setupGoalState(RKPPlannerSetup *psetup, motion_planning_srvs::KinematicPlan::Request &req);
-
- /** Configure the state space for a given set of parameters and set the starting state */
- void setupStateSpaceAndStartState(RKPPlannerSetup *psetup,
- motion_planning_msgs::KinematicSpaceParameters ¶ms,
- motion_planning_msgs::KinematicState &start_state);
-
- /** Set the kinematic constraints to follow */
- void setupConstraints(RKPPlannerSetup *psetup, const motion_planning_msgs::KinematicConstraints &cstrs);
-
/** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
- bool computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
+ bool callPlanner(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
ompl::sb::PathKinematic* &bestPath, double &bestDifference, bool &approximate);
- void fillSolution(RKPPlannerSetup *psetup, ompl::sb::PathKinematic *bestPath, double bestDifference,
- motion_planning_msgs::KinematicPath &path, double &distance);
- void cleanupPlanningData(RKPPlannerSetup *psetup);
- motion_planning_srvs::KinematicPlan::Request m_activeReq;
- motion_planning_msgs::KinematicState m_activeStartState;
- RKPPlannerSetup *m_activePSetup;
};
} // kinematic_planning
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPProjectionEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPProjectionEvaluators.h 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPProjectionEvaluators.h 2009-06-14 00:59:35 UTC (rev 17064)
@@ -50,8 +50,8 @@
LinkPositionProjectionEvaluator(RKPModelBase *model, const std::string &linkName) : ompl::base::ProjectionEvaluator()
{
- m_model = model;
- m_link = m_model->kmodel->getLink(linkName);
+ model_ = model;
+ link_ = model_->kmodel->getLink(linkName);
}
/** Return the dimension of the projection defined by this evaluator */
@@ -64,19 +64,17 @@
virtual void operator()(const ompl::base::State *state, double *projection) const
{
const ompl::sb::State *kstate = static_cast<const ompl::sb::State*>(state);
- m_model->kmodel->lock();
- m_model->kmodel->computeTransformsGroup(kstate->values, m_model->groupID);
- const btVector3 &origin = m_link->globalTrans.getOrigin();
+ model_->kmodel->computeTransformsGroup(kstate->values, model_->groupID);
+ const btVector3 &origin = link_->globalTrans.getOrigin();
projection[0] = origin.x();
projection[1] = origin.y();
projection[2] = origin.z();
- m_model->kmodel->unlock();
}
protected:
- RKPModelBase *m_model;
- planning_models::KinematicModel::Link *m_link;
+ RKPModelBase *model_;
+ planning_models::KinematicModel::Link *link_;
};
Modified: pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/ompl_planning/include/kinematic_planning/ompl_extensions/RKPStateValidator.h 2009-06-14 00:59:35 UTC (rev 17064)
@@ -58,10 +58,9 @@
virtual bool operator()(const ompl::base::State *s) const
{
- model_->kmodel->lock();
const double *state = static_cast<const ompl::sb::State*>(s)->values;
model_->kmodel->computeTransformsGroup(state, model_->groupID);
-
+
bool valid = kce_.decide(state, model_->groupID);
if (valid)
{
@@ -69,8 +68,6 @@
valid = !model_->collisionSpace->isCollision();
}
- model_->kmodel->unlock();
-
return valid;
}
@@ -94,7 +91,8 @@
}
protected:
- mutable RKPModelBase *model_;
+
+ RKPModelBase *model_;
SpaceInformationRKPModel *si_;
planning_environment::KinematicConstraintEvaluatorSet kce_;
};
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-13 04:44:58 UTC (rev 17063)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RKPRequestHandler.cpp 2009-06-14 00:59:35 UTC (rev 17064)
@@ -35,229 +35,73 @@
/** \author Ioan Sucan */
#include "kinematic_planning/RKPRequestHandler.h"
+#include <ros/console.h>
+#include <sstream>
-/** Set up all the data needed by motion planning based on a request and lock the planner setup
- * using this data */
-bool kinematic_planning::RKPRequestHandler::configure(ModelMap &models, const motion_planning_msgs::KinematicState &startState, const motion_planning_srvs::KinematicPlan::Request &req)
-{
- // make sure the same motion planner instance is not used by other calls
- m_activeReq = req;
- m_activeStartState = startState;
-
- if (!isRequestValid(models, m_activeStartState, m_activeReq))
- return false;
-
- ROS_INFO("Selected motion planner: '%s'", m_activeReq.params.planner_id.c_str());
-
- /* find the data we need */
- RKPModel *m = models[m_activeReq.params.model_id];
-
- // lock the model
- m->lock.lock();
-
- // get the planner setup
- m_activePSetup = m->planners[m_activeReq.params.planner_id];
-
- update();
-
- /* print some information */
- ROS_DEBUG("=======================================");
- std::stringstream ss;
- m_activePSetup->si->printSettings(ss);
- ss << "Path constraints:" << std::endl;
- static_cast<StateValidityPredicate*>(m_activePSetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
- ROS_DEBUG("%s", ss.str().c_str());
- ROS_DEBUG("=======================================");
-
- /* clear memory */
- cleanupPlanningData(m_activePSetup);
-
- return true;
-}
-
-void kinematic_planning::RKPRequestHandler::release(void)
-{
- if (m_activePSetup)
- {
- m_activePSetup->model->lock.unlock();
- m_activePSetup = NULL;
- }
-}
-
-bool kinematic_planning::RKPRequestHandler::isActive(void)
-{
- return m_activePSetup != NULL;
-}
-
-kinematic_planning::RKPPlannerSetup *kinematic_planning::RKPRequestHandler::activePlannerSetup(void)
-{
- return m_activePSetup;
-}
-
-motion_planning_srvs::KinematicPlan::Request& kinematic_planning::RKPRequestHandler::activeRequest(void)
-{
- return m_activeReq;
-}
-
-motion_planning_msgs::KinematicState& kinematic_planning::RKPRequestHandler::activeStartState(void)
-{
- return m_activeStartState;
-}
-
-bool kinematic_planning::RKPRequestHandler::isStillValid(motion_planning_msgs::KinematicPath &path)
-{
- update();
-
- /* copy the path msg into a kinematic path structure (ompl style) */
- ompl::sb::PathKinematic *kpath = new ompl::sb::PathKinematic(m_activePSetup->si);
- unsigned int dim = m_activePSetup->si->getStateDimension();
- for (unsigned int i = 0 ; i < path.states.size() ; ++i)
- {
- ompl::sb::State *kstate = new ompl::sb::State(dim);
- kpath->states.push_back(kstate);
- for (unsigned int j = 0 ; j < dim ; ++j)
- kstate->values[j] = path.states[i].vals[j];
- }
-
- m_activePSetup->model->collisionSpace->lock();
- bool valid = m_activePSetup->si->checkPath(kpath);
- m_activePSetup->model->collisionSpace->unlock();
-
- delete kpath;
-
- /* clear memory */
- cleanupPlanningData(m_activePSetup);
-
- return valid;
-}
-
-bool kinematic_planning::RKPRequestHandler::isTrivial(double *distance)
-{
- update();
-
- m_activePSetup->model->collisionSpace->lock();
- bool trivial = m_activePSetup->mp->isTrivial(NULL, distance);
- m_activePSetup->model->collisionSpace->unlock();
-
- /* clear memory */
- cleanupPlanningData(m_activePSetup);
-
- return trivial;
-}
-
-void kinematic_planning::RKPRequestHandler::execute(motion_planning_msgs::KinematicPath &path, double &distance, bool &trivial, bool &approximate)
-{
- update();
-
- /* compute actual motion plan */
- ompl::sb::PathKinematic *bestPath = NULL;
- double bestDifference = 0.0;
-
- m_activePSetup->model->collisionSpace->lock();
- trivial = computePlan(m_activePSetup, m_activeReq.times, m_activeReq.allowed_time, true, bestPath, bestDifference, approximate);
- m_activePSetup->model->collisionSpace->unlock();
-
- /* fill in the results */
- fillSolution(m_activePSetup, bestPath, bestDifference, path, distance);
-
- /* clear memory */
- cleanupPlanningData(m_activePSetup);
-}
-
-
/** Check if request looks valid */
bool kinematic_planning::RKPRequestHandler::isRequestValid(ModelMap &models, motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req)
-{
- if (!areSpaceParamsValid(models, req.params))
- return false;
+{
+ ModelMap::const_iterator pos = models.find(req.params.model_id);
- RKPModel *m = models[req.params.model_id];
-
- if (m->kmodel->getModelInfo().stateDimension != startState.get_vals_size())
+ if (pos == models.end())
{
- ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, startState.get_vals_size());
+ ROS_ERROR("Cannot plan for '%s'. Model does not exist", req.params.model_id.c_str());
return false;
}
- return true;
-}
-
-/* Set the goal */
-void kinematic_planning::RKPRequestHandler::setupGoalState(RKPPlannerSetup *psetup, motion_planning_srvs::KinematicPlan::Request &req)
-{
- psetup->si->setGoal(computeGoalFromConstraints(psetup->si, psetup->model, req.goal_constraints));
-}
-
-void kinematic_planning::RKPRequestHandler::update(void)
-{
- /* before configuring, we may need to update bounds on the state space */
- setupConstraints(m_activePSetup, m_activeReq.path_constraints);
-
- /* configure state space and starting state */
- setupStateSpaceAndStartState(m_activePSetup, m_activeReq.params, m_activeStartState);
-
- /* add goal state */
- setupGoalState(m_activePSetup, m_activeReq);
-
- std::vector<double> rho(m_activePSetup->si->getStateDimension(), 0.1);
- m_activePSetup->si->fixInvalidInputStates(rho, rho, 100);
-}
-
-/** Validate common space parameters */
-bool kinematic_planning::RKPRequestHandler::areSpaceParamsValid(const ModelMap &modelsRef, motion_planning_msgs::KinematicSpaceParameters ¶ms) const
-{
- ModelMap::const_iterator pos = modelsRef.find(params.model_id);
-
- if (pos == modelsRef.end())
- {
- ROS_ERROR("Cannot plan for '%s'. Model does not exist", params.model_id.c_str());
- return false;
- }
-
/* find the model */
RKPModel *m = pos->second;
/* if the user did not specify a planner, use the first available one */
- if (params.planner_id.empty())
+ if (req.params.planner_id.empty())
{
if (!m->planners.empty())
- params.planner_id = m->planners.begin()->first;
+ req.params.planner_id = m->planners.begin()->first;
}
-
+
/* check if desired planner exists */
- std::map<std::string, RKPPlannerSetup*>::iterator plannerIt = m->planners.find(params.planner_id);
+ std::map<std::string, RKPPlannerSetup*>::iterator plannerIt = m->planners.find(req.params.planner_id);
if (plannerIt == m->planners.end())
{
- ROS_ERROR("Motion planner not found: '%s'", params.planner_id.c_str());
+ ROS_ERROR("Motion planner not found: '%s'", req.params.planner_id.c_str());
return false;
}
RKPPlannerSetup *psetup = plannerIt->second;
/* check if the desired distance metric is defined */
- if (psetup->sde.find(params.distance_metric) == psetup->sde.end())
+ if (psetup->sde.find(req.params.distance_metric) == psetup->sde.end())
{
- ROS_ERROR("Distance evaluator not found: '%s'", params.distance_metric.c_str());
+ ROS_ERROR("Distance evaluator not found: '%s'", req.params.distance_metric.c_str());
return false;
}
+ if (m->kmodel->getModelInfo().stateDimension != startState.get_vals_size())
+ {
+ ROS_ERROR("Dimension of start state expected to be %d but was received as %d", m->kmodel->getModelInfo().stateDimension, startState.get_vals_size());
+ return false;
+ }
+
return true;
}
-/** Configure the state space for a given set of parameters and set the starting state */
-void kinematic_planning::RKPRequestHandler::setupStateSpaceAndStartState(RKPPlannerSetup *psetup,
- motion_planning_msgs::KinematicSpaceParameters ¶ms,
- motion_planning_msgs::KinematicState &start_state)
+void kinematic_planning::RKPRequestHandler::configure(const motion_planning_msgs::KinematicState &startState, motion_planning_srvs::KinematicPlan::Request &req, RKPPlannerSetup *psetup)
{
+ /* clear memory */
+ psetup->si->clearGoal();
+ psetup->si->clearStartStates();
+
+ /* before configuring, we may need to update bounds on the state space */
+ static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setConstraints(req.path_constraints);
+
/* set the workspace volume for planning */
- // only area or volume should go through... not sure what happens on really complicated models where
- // we have both multiple planar and multiple floating joints... it might work :)
- static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningArea(params.volumeMin.x, params.volumeMin.y,
- params.volumeMax.x, params.volumeMax.y);
- static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningVolume(params.volumeMin.x, params.volumeMin.y, params.volumeMin.z,
- params.volumeMax.x, params.volumeMax.y, params.volumeMax.z);
+ // only area or volume should go through
+ static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningArea(req.params.volumeMin.x, req.params.volumeMin.y,
+ req.params.volumeMax.x, req.params.volumeMax.y);
+ static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningVolume(req.params.volumeMin.x, req.params.volumeMin.y, req.params.volumeMin.z,
+ req.params.volumeMax.x, req.params.volumeMax.y, req.params.volumeMax.z);
- psetup->si->setStateDistanceEvaluator(psetup->sde[params.distance_metric]);
+ psetup->si->setStateDistanceEvaluator(psetup->sde[req.params.distance_metric]);
/* set the starting state */
const unsigned int dim = psetup->si->getStateDimension();
@@ -266,34 +110,113 @@
if (psetup->model->groupID >= 0)
{
/* set the pose of the whole robot */
- psetup->model->kmodel->computeTransforms(&start_state.vals[0]);
+ psetup->model->kmodel->computeTransforms(&startState.vals[0]);
psetup->model->collisionSpace->updateRobotModel();
/* extract the components needed for the start state of the desired group */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = start_state.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
+ start->values[i] = startState.vals[psetup->model->kmodel->getModelInfo().groupStateIndexList[psetup->model->groupID][i]];
}
else
{
/* the start state is the complete state */
for (unsigned int i = 0 ; i < dim ; ++i)
- start->values[i] = start_state.vals[i];
+ start->values[i] = startState.vals[i];
}
psetup->si->addStartState(start);
+
+ /* add goal state */
+ psetup->si->setGoal(computeGoalFromConstraints(psetup->si, psetup->model, req.goal_constraints));
+
+ /* add bounds for automatic state fixing (in case a state is invalid) */
+ std::vector<double> rhoStart(psetup->si->getStateDimension(), 0.05);
+ std::vector<double> rhoGoal(rhoStart);
+ // in case we have large bounds, we may have a larger area to sample,
+ // so we increase it if we can
+ GoalToState *gs = dynamic_cast<GoalToState*>(psetup->si->getGoal());
+ if (gs)
+ {
+ std::vector< std::pair<double, double> > bounds = gs->getBounds();
+ for (unsigned int i = 0 ; i < rhoGoal.size() ; ++i)
+ {
+ double dif = bounds[i].second - bounds[i].first;
+ if (dif > rhoGoal[i])
+ rhoGoal[i] = dif;
+ }
+ }
+ psetup->si->fixInvalidInputStates(rhoStart, rhoGoal, 100);
+
+ /* print some information */
+ ROS_DEBUG("=======================================");
+ std::stringstream ss;
+ psetup->si->printSettings(ss);
+ ss << "Path constraints:" << std::endl;
+ static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->getKinematicConstraintEvaluatorSet().print(ss);
+ ROS_DEBUG("%s", ss.str().c_str());
+ ROS_DEBUG("=======================================");
}
-/** Set the kinematic constraints to follow */
-void kinematic_planning::RKPRequestHandler::setupConstraints(RKPPlannerSetup *psetup, const motion_planning_msgs::KinematicConstraints &cstrs)
+bool kinematic_planning::RKPRequestHandler::computePlan(ModelMap &models, motion_planning_srvs::KinematicPlan::Request &req, motion_planning_srvs::KinematicPlan::Response &res)
{
- static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setConstraints(cstrs);
-}
+ if (!isRequestValid(models, res.path.start_state, req))
+ return false;
+
+ ROS_INFO("Selected motion planner: '%s'", req.params.planner_id.c_str());
+
+ // find the data we need
+ RKPModel *m = models[req.params.model_id];
+
+ // get the planner setup
+ RKPPlannerSetup *psetup = m->planners[req.params.planner_id];
+
+ // configure the planner
+ configure(res.path.start_state, req, psetup);
+
+ /* compute actual motion plan */
+ ompl::sb::PathKinematic *bestPath = NULL;
+ double bestDifference = 0.0;
+
+ psetup->model->collisionSpace->lock();
+ psetup->model->kmodel->lock();
+
+ bool approximate = false;
+ callPlanner(psetup, req.times, req.allowed_time, true, bestPath, bestDifference, approximate);
+
+ psetup->model->collisionSpace->unlock();
+ psetup->model->kmodel->unlock();
+ psetup->si->clearGoal();
+ psetup->si->clearStartStates();
+
+ /* copy the solution to the result */
+ if (bestPath)
+ {
+ res.path.states.resize(bestPath->states.size());
+ res.path.times.resize(bestPath->states.size());
+ res.path.names.clear();
+ psetup->model->kmodel->getJointsInGroup(res.path.names, psetup->model->groupID);
+
+ const unsigned int dim = psetup->si->getStateDimension();
+ for (unsigned int i = 0 ; i < bestPath->states.size() ; ++i)
+ {
+ res.path.times[i] = i * 0.02;
+ res.path.states[i].vals.resize(dim);
+ for (unsigned int j = 0 ; j < dim ; ++j)
+ res.path.states[i].vals[j] = bestPath->states[i]->values[j];
+ }
+ res.distance = bestDifference;
+ res.approximate = approximate ? 1 : 0;
+ delete bestPath;
+ }
+
+ return true;
+}
+
/** Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
-bool kinematic_planning::RKPRequestHandler::computePlan(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
+bool kinematic_planning::RKPRequestHandler::callPlanner(RKPPlannerSetup *psetup, int times, double allowed_time, bool interpolate,
ompl::sb::PathKinematic* &bestPath, double &bestDifference, bool &approximate)
{
-
if (times <= 0)
{
ROS_ERROR("Motion plan cannot be computed %d times", times);
@@ -370,40 +293,3 @@
}
return result;
}
-
-void kinematic_planning::RKPRequestHandler::fillSolution(RKPPlannerSetup *psetup, ompl::sb::PathKinematic *bestPath, double bestDifference,
- motion_planning_msgs::KinematicPath &path, double &distance)
-{
- const unsigned int dim = psetup->si->getStateDimension();
-
- /* copy the solution to the result */
- if (bestPath)
- {
- path.states.resize(bestPath->states.size());
- path.times.resize(bestPath->states.size());
- path.names.clear();
- psetup->model->kmodel->getJointsInGroup(path.names, psetup->model->groupID);
-
- for (unsigned int i = 0 ; i < bestPath->states.size() ; ++i)
- {
- path.times[i] = i * 0.1;
- path.states[i].vals.resize(dim);
- for (unsigned int j = 0 ; j < dim ; ++j)
- path.states[i].vals[j] = bestPath->states[i]->values[j];
- }
- distance = bestDifference;
- delete bestPath;
- }
- else
- {
- path.set_states_size(0);
- distance = -1.0;
- }
-}
-
-void kinematic_planning::RKPRequestHandler::cleanupPlanningData(RKPPlannerSetup *psetup)
-{
- /* cleanup */
- psetup->si->clearGoal();
- psetup->si->clearStartStates();
-}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/t...
[truncated message content] |
|
From: <is...@us...> - 2009-06-14 01:04:16
|
Revision: 17065
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17065&view=rev
Author: isucan
Date: 2009-06-14 01:04:15 +0000 (Sun, 14 Jun 2009)
Log Message:
-----------
using openmp flag
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-14 00:59:35 UTC (rev 17064)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-14 01:04:15 UTC (rev 17065)
@@ -11,7 +11,7 @@
src/collision_space_monitor.cpp
src/kinematic_state_constraint_evaluator.cpp
src/planning_monitor.cpp)
-#rospack_add_openmp_flags(planning_environment)
+rospack_add_openmp_flags(planning_environment)
# Tests
Modified: pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-14 00:59:35 UTC (rev 17064)
+++ pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-14 01:04:15 UTC (rev 17065)
@@ -28,4 +28,4 @@
#rospack_link_boost(${PROJECT_NAME} thread)
rospack_add_executable(test_filter test_filter.cpp)
-#rospack_add_openmp_flags(test_filter)
+rospack_add_openmp_flags(test_filter)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-06-16 02:22:13
|
Revision: 17122
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17122&view=rev
Author: isucan
Date: 2009-06-16 01:36:01 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
renaming robot self filter using openrave
Modified Paths:
--------------
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/manifest.xml
pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
pkg/trunk/util/new_robot_self_filter/manifest.xml
pkg/trunk/util/or_robot_self_filter/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/util/new_robot_self_filter/include/
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/
pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/src/
pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
pkg/trunk/util/or_robot_self_filter/
Removed Paths:
-------------
pkg/trunk/util/new_robot_self_filter/self_see_filter.h
pkg/trunk/util/new_robot_self_filter/test_filter.cpp
pkg/trunk/util/robot_self_filter/
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-06-16 01:36:01 UTC (rev 17122)
@@ -20,6 +20,6 @@
<depend package="pr2_prototype1_gazebo"/>
<depend package="point_cloud_assembler"/>
<depend package="semantic_point_annotator"/>
- <depend package="robot_self_filter"/>
+ <depend package="or_robot_self_filter"/>
<depend package="rviz"/>
</package>
Modified: pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml 2009-06-16 01:36:01 UTC (rev 17122)
@@ -16,7 +16,7 @@
<depend package="nav_view"/>
<depend package="stage"/>
<depend package="pr2_gazebo"/>
- <depend package="robot_self_filter"/>
+ <depend package="or_robot_self_filter"/>
<depend package="point_cloud_assembler"/>
<depend package="collision_map"/>
<depend package="laser_scan"/>
Modified: pkg/trunk/openrave_planning/ormanipulation/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/manifest.xml 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/openrave_planning/ormanipulation/manifest.xml 2009-06-16 01:36:01 UTC (rev 17122)
@@ -8,6 +8,6 @@
<depend package="orplugins"/>
<depend package="roslaunch"/>
<depend package="laser_scan"/>
- <depend package="robot_self_filter"/>
+ <depend package="or_robot_self_filter"/>
<depend package="collision_map"/>
</package>
Modified: pkg/trunk/util/new_robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/util/new_robot_self_filter/CMakeLists.txt 2009-06-16 01:36:01 UTC (rev 17122)
@@ -11,21 +11,8 @@
rospack(new_robot_self_filter)
-#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(test_filter test_filter.cpp)
+rospack_add_executable(test_filter src/test_filter.cpp)
rospack_add_openmp_flags(test_filter)
+
+rospack_add_executable(self_filter src/self_filter.cpp)
+rospack_add_openmp_flags(self_filter)
Copied: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h (from rev 17097, pkg/trunk/util/new_robot_self_filter/self_see_filter.h)
===================================================================
--- pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h 2009-06-16 01:36:01 UTC (rev 17122)
@@ -0,0 +1,235 @@
+/*
+ * 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, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FILTERS_SELF_SEE_H_
+#define FILTERS_SELF_SEE_H_
+
+#include <filters/filter_base.h>
+#include <ros/console.h>
+#include <robot_msgs/PointCloud.h>
+#include <planning_environment/robot_models.h>
+#include <collision_space/bodies.h>
+#include <tf/transform_listener.h>
+#include <string>
+#include <algorithm>
+
+namespace filters
+{
+
+/** \brief A filter to remove parts of the robot seen in a pointcloud
+ *
+ */
+
+template <typename T>
+class SelfFilter: public FilterBase <T>
+{
+
+protected:
+
+ struct SeeLink
+ {
+ std::string name;
+ collision_space::bodies::Body* body;
+ btTransform constTransf;
+ };
+
+ struct SortBodies
+ {
+ bool operator()(const SeeLink &b1, const SeeLink &b2)
+ {
+ return b1.body->computeVolume() > b2.body->computeVolume();
+ }
+ };
+
+public:
+ /** \brief Construct the filter */
+ SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
+ {
+ tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
+ }
+
+
+ /** \brief Destructor to clean up
+ */
+ ~SelfFilter(void)
+ {
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ if (bodies_[i].body)
+ delete bodies_[i].body;
+ }
+
+ virtual bool configure(void)
+ {
+ // keep only the points that are outside of the robot
+ // for testing purposes this may be changed to true
+ invert_ = false;
+
+ if (invert_)
+ ROS_WARN("Inverting filter output");
+
+ std::vector<std::string> links = rm_.getSelfSeeLinks();
+ double scale = rm_.getSelfSeeScale();
+ double padd = rm_.getSelfSeePadding();
+
+ // from the geometric model, find the shape of each link of interest
+ // and create a body from it, one that knows about poses and can
+ // check for point inclusion
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ SeeLink sl;
+ sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
+ if (sl.body)
+ {
+ sl.name = links[i];
+
+ // collision models may have an offset, in addition to what TF gives
+ // so we keep it around
+ sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
+ sl.body->setScale(scale);
+ sl.body->setPadding(padd);
+ bodies_.push_back(sl);
+ }
+ else
+ ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+ }
+
+ if (bodies_.empty())
+ ROS_WARN("No robot links will be checked for self collision");
+
+ // put larger volume bodies first -- higher chances of containing a point
+ std::sort(bodies_.begin(), bodies_.end(), SortBodies());
+
+ for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
+ ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
+
+ return true;
+ }
+
+
+ /** \brief Update the filter and return the data seperately
+ * \param data_in T array with length width
+ * \param data_out T array with length width
+ */
+ virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
+ {
+ if (bodies_.empty())
+ data_out = data_in;
+ else
+ {
+ const unsigned int bs = bodies_.size();
+ const unsigned int np = data_in.pts.size();
+ std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
+
+ // place the links in the frame of the pointcloud
+ for (unsigned int i = 0 ; i < bs ; ++i)
+ {
+ // find the transform between the link's frame and the pointcloud frame
+ tf::Stamped<btTransform> transf;
+ if (tf_.canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
+ tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
+ else
+ {
+ transf.setIdentity();
+ ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
+ }
+
+ // set it for each body; we also include the offset specified in URDF
+ bodies_[i].body->setPose(transf * bodies_[i].constTransf);
+ bodies_[i].body->computeBoundingSphere(bspheres[i]);
+ }
+
+ // compute a sphere that bounds the entire robot
+ collision_space::bodies::BoundingSphere bound;
+ collision_space::bodies::mergeBoundingSpheres(bspheres, bound);
+
+ // we now decide which points we keep
+ std::vector<bool> keep(np);
+
+#pragma omp parallel for
+ for (int i = 0 ; i < (int)np ; ++i)
+ {
+ btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
+ bool out = true;
+ if (bound.center.distance(pt) < bound.radius)
+ for (unsigned int j = 0 ; out && j < bs ; ++j)
+ out = !bodies_[j].body->containsPoint(pt);
+
+ keep[i] = invert_ ? !out : out;
+ }
+
+
+ // fill in output data
+ data_out.header = data_in.header;
+
+ data_out.pts.resize(0);
+ data_out.pts.reserve(np);
+
+ data_out.chan.resize(data_in.chan.size());
+ for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
+ {
+ ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
+ data_out.chan[i].name = data_in.chan[i].name;
+ data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
+ }
+
+ for (unsigned int i = 0 ; i < np ; ++i)
+ if (keep[i])
+ {
+ data_out.pts.push_back(data_in.pts[i]);
+ for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
+ data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
+ }
+ }
+ return true;
+ }
+
+ virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
+ {
+ data_out.resize(data_in.size());
+ for (unsigned int i = 0 ; i < data_in.size() ; ++i)
+ update(data_in[i], data_out[i]);
+ return true;
+ }
+
+protected:
+
+ planning_environment::RobotModels rm_;
+ ros::NodeHandle nh_;
+ tf::TransformListener tf_;
+ std::vector<SeeLink> bodies_;
+ bool invert_;
+
+};
+
+typedef robot_msgs::PointCloud robot_msgs_PointCloud;
+FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
+
+}
+
+#endif //#ifndef FILTERS_SELF_SEE_H_
Property changes on: pkg/trunk/util/new_robot_self_filter/include/robot_self_filter/self_see_filter.h
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/util/new_robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/util/new_robot_self_filter/manifest.xml 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/util/new_robot_self_filter/manifest.xml 2009-06-16 01:36:01 UTC (rev 17122)
@@ -17,6 +17,10 @@
<depend package="collision_space"/>
<depend package="planning_environment"/>
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="" />
+ </export>
+
</package>
Deleted: pkg/trunk/util/new_robot_self_filter/self_see_filter.h
===================================================================
--- pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/util/new_robot_self_filter/self_see_filter.h 2009-06-16 01:36:01 UTC (rev 17122)
@@ -1,235 +0,0 @@
-/*
- * 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, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef FILTERS_SELF_SEE_H_
-#define FILTERS_SELF_SEE_H_
-
-#include <filters/filter_base.h>
-#include <ros/console.h>
-#include <robot_msgs/PointCloud.h>
-#include <planning_environment/robot_models.h>
-#include <collision_space/bodies.h>
-#include <tf/transform_listener.h>
-#include <string>
-#include <algorithm>
-
-namespace filters
-{
-
-/** \brief A filter to remove parts of the robot seen in a pointcloud
- *
- */
-
-template <typename T>
-class SelfFilter: public FilterBase <T>
-{
-
-protected:
-
- struct SeeLink
- {
- std::string name;
- collision_space::bodies::Body* body;
- btTransform constTransf;
- };
-
- struct SortBodies
- {
- bool operator()(const SeeLink &b1, const SeeLink &b2)
- {
- return b1.body->computeVolume() > b2.body->computeVolume();
- }
- };
-
-public:
- /** \brief Construct the filter */
- SelfFilter(void) : rm_("robot_description"), tf_(*ros::Node::instance(), true, ros::Duration(10))
- {
- tf_.setExtrapolationLimit(ros::Duration().fromSec(10));
- }
-
-
- /** \brief Destructor to clean up
- */
- ~SelfFilter(void)
- {
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- if (bodies_[i].body)
- delete bodies_[i].body;
- }
-
- virtual bool configure(void)
- {
- // keep only the points that are outside of the robot
- // for testing purposes this may be changed to true
- invert_ = false;
-
- if (invert_)
- ROS_WARN("Inverting filter output");
-
- std::vector<std::string> links = rm_.getSelfSeeLinks();
- double scale = rm_.getSelfSeeScale();
- double padd = rm_.getSelfSeePadding();
-
- // from the geometric model, find the shape of each link of interest
- // and create a body from it, one that knows about poses and can
- // check for point inclusion
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- {
- SeeLink sl;
- sl.body = collision_space::bodies::createBodyFromShape(rm_.getKinematicModel()->getLink(links[i])->shape);
- if (sl.body)
- {
- sl.name = links[i];
-
- // collision models may have an offset, in addition to what TF gives
- // so we keep it around
- sl.constTransf = rm_.getKinematicModel()->getLink(links[i])->constGeomTrans;
- sl.body->setScale(scale);
- sl.body->setPadding(padd);
- bodies_.push_back(sl);
- }
- else
- ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
- }
-
- if (bodies_.empty())
- ROS_WARN("No robot links will be checked for self collision");
-
- // put larger volume bodies first -- higher chances of containing a point
- std::sort(bodies_.begin(), bodies_.end(), SortBodies());
-
- for (unsigned int i = 0 ; i < bodies_.size() ; ++i)
- ROS_INFO("Self see link %s with volume %f", bodies_[i].name.c_str(), bodies_[i].body->computeVolume());
-
- return true;
- }
-
-
- /** \brief Update the filter and return the data seperately
- * \param data_in T array with length width
- * \param data_out T array with length width
- */
- virtual bool update(const robot_msgs::PointCloud& data_in, robot_msgs::PointCloud& data_out)
- {
- if (bodies_.empty())
- data_out = data_in;
- else
- {
- const unsigned int bs = bodies_.size();
- const unsigned int np = data_in.pts.size();
- std::vector<collision_space::bodies::BoundingSphere> bspheres(bs);
-
- // place the links in the frame of the pointcloud
- for (unsigned int i = 0 ; i < bs ; ++i)
- {
- // find the transform between the link's frame and the pointcloud frame
- tf::Stamped<btTransform> transf;
- if (tf_.canTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp))
- tf_.lookupTransform(data_in.header.frame_id, bodies_[i].name, data_in.header.stamp, transf);
- else
- {
- transf.setIdentity();
- ROS_ERROR("Unable to lookup transform from %s to %s", bodies_[i].name.c_str(), data_in.header.frame_id.c_str());
- }
-
- // set it for each body; we also include the offset specified in URDF
- bodies_[i].body->setPose(transf * bodies_[i].constTransf);
- bodies_[i].body->computeBoundingSphere(bspheres[i]);
- }
-
- // compute a sphere that bounds the entire robot
- collision_space::bodies::BoundingSphere bound;
- collision_space::bodies::mergeBoundingSpheres(bspheres, bound);
-
- // we now decide which points we keep
- std::vector<bool> keep(np);
-
-#pragma omp parallel for
- for (int i = 0 ; i < (int)np ; ++i)
- {
- btVector3 pt = btVector3(btScalar(data_in.pts[i].x), btScalar(data_in.pts[i].y), btScalar(data_in.pts[i].z));
- bool out = true;
- if (bound.center.distance(pt) < bound.radius)
- for (unsigned int j = 0 ; out && j < bs ; ++j)
- out = !bodies_[j].body->containsPoint(pt);
-
- keep[i] = invert_ ? !out : out;
- }
-
-
- // fill in output data
- data_out.header = data_in.header;
-
- data_out.pts.resize(0);
- data_out.pts.reserve(np);
-
- data_out.chan.resize(data_in.chan.size());
- for (unsigned int i = 0 ; i < data_out.chan.size() ; ++i)
- {
- ROS_ASSERT(data_in.chan[i].vals.size() == data_in.pts.size());
- data_out.chan[i].name = data_in.chan[i].name;
- data_out.chan[i].vals.reserve(data_in.chan[i].vals.size());
- }
-
- for (unsigned int i = 0 ; i < np ; ++i)
- if (keep[i])
- {
- data_out.pts.push_back(data_in.pts[i]);
- for (unsigned int j = 0 ; j < data_out.chan.size() ; ++j)
- data_out.chan[j].vals.push_back(data_in.chan[j].vals[i]);
- }
- }
- return true;
- }
-
- virtual bool update(const std::vector<robot_msgs::PointCloud> & data_in, std::vector<robot_msgs::PointCloud>& data_out)
- {
- data_out.resize(data_in.size());
- for (unsigned int i = 0 ; i < data_in.size() ; ++i)
- update(data_in[i], data_out[i]);
- return true;
- }
-
-protected:
-
- planning_environment::RobotModels rm_;
- ros::NodeHandle nh_;
- tf::TransformListener tf_;
- std::vector<SeeLink> bodies_;
- bool invert_;
-
-};
-
-typedef robot_msgs::PointCloud robot_msgs_PointCloud;
-FILTERS_REGISTER_FILTER(SelfFilter, robot_msgs_PointCloud);
-
-}
-
-#endif //#ifndef FILTERS_SELF_SEE_H_
Added: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp 2009-06-16 01:36:01 UTC (rev 17122)
@@ -0,0 +1,78 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include <ros/ros.h>
+#include "robot_self_filter/self_see_filter.h"
+
+class SelfFilter
+{
+public:
+
+ SelfFilter(void)
+ {
+ sf_.configure();
+ pointCloudSubscriber_ = nh_.subscribe("full_cloud", 1, &SelfFilter::cloudCallback, this);
+ pointCloudPublisher_ = nh_.advertise<robot_msgs::PointCloud>("full_cloud_filtered", 1);
+ }
+
+ void cloudCallback(const robot_msgs::PointCloudConstPtr &cloud)
+ {
+ robot_msgs::PointCloud out;
+ sf_.update(*cloud, out);
+ pointCloudPublisher_.publish(out);
+ }
+
+private:
+
+ ros::Publisher pointCloudPublisher_;
+ ros::Subscriber pointCloudSubscriber_;
+ ros::NodeHandle nh_;
+ filters::SelfFilter<robot_msgs::PointCloud> sf_;
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "self_filter");
+
+ SelfFilter s;
+ ros::spin();
+
+ return 0;
+}
+
+
+
Property changes on: pkg/trunk/util/new_robot_self_filter/src/self_filter.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
Copied: pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp (from rev 17119, pkg/trunk/util/new_robot_self_filter/test_filter.cpp)
===================================================================
--- pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp (rev 0)
+++ pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp 2009-06-16 01:36:01 UTC (rev 17122)
@@ -0,0 +1,133 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
+#include "robot_self_filter/self_see_filter.h"
+
+class TestSelfFilter
+{
+public:
+
+ TestSelfFilter(void)
+ {
+ id_ = 1;
+ sf_.configure();
+ vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
+ }
+
+ void sendPoint(double x, double y, double z)
+ {
+ visualization_msgs::Marker mk;
+
+ mk.header.stamp = ros::Time::now();
+
+ mk.header.frame_id = "base_link";
+
+ mk.ns = "test_self_filter";
+ mk.id = id_++;
+ mk.type = visualization_msgs::Marker::SPHERE;
+ mk.action = visualization_msgs::Marker::ADD;
+ mk.pose.position.x = x;
+ mk.pose.position.y = y;
+ mk.pose.position.z = z;
+ mk.pose.orientation.w = 1.0;
+
+ mk.scale.x = mk.scale.y = mk.scale.z = 0.02;
+
+ mk.color.a = 1.0;
+ mk.color.r = 1.0;
+ mk.color.g = 0.04;
+ mk.color.b = 0.04;
+
+ vmPub_.publish(mk);
+ }
+
+ void run(void)
+ {
+ robot_msgs::PointCloud in;
+ robot_msgs::PointCloud out;
+
+ in.header.stamp = ros::Time::now();
+ in.header.frame_id = "base_link";
+
+ const unsigned int N = 100000;
+ in.pts.resize(N);
+ for (unsigned int i = 0 ; i < N ; ++i)
+ {
+ in.pts[i].x = uniform(1);
+ in.pts[i].y = uniform(1);
+ in.pts[i].z = uniform(1);
+ }
+
+ ros::WallTime tm = ros::WallTime::now();
+ sf_.update(in, out);
+ printf("%f points per second\n", (double)N/(ros::WallTime::now() - tm).toSec());
+
+ for (unsigned int i = 0 ; i < out.pts.size() ; ++i)
+ {
+ sendPoint(out.pts[i].x, out.pts[i].y, out.pts[i].z);
+ }
+
+ ros::spin();
+ }
+protected:
+
+ double uniform(double magnitude)
+ {
+ return (2.0 * drand48() - 1.0) * magnitude;
+ }
+
+ ros::Publisher vmPub_;
+ ros::NodeHandle nodeHandle_;
+ filters::SelfFilter<robot_msgs::PointCloud> sf_;
+ int id_;
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "test_self_filter");
+
+ TestSelfFilter t;
+ sleep(1);
+ t.run();
+
+ return 0;
+}
+
+
+
Property changes on: pkg/trunk/util/new_robot_self_filter/src/test_filter.cpp
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/util/new_robot_self_filter/test_filter.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/util/new_robot_self_filter/test_filter.cpp
===================================================================
--- pkg/trunk/util/new_robot_self_filter/test_filter.cpp 2009-06-16 01:31:53 UTC (rev 17121)
+++ pkg/trunk/util/new_robot_self_filter/test_filt...
[truncated message content] |
|
From: <hsu...@us...> - 2009-06-16 03:02:11
|
Revision: 17129
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17129&view=rev
Author: hsujohnhsu
Date: 2009-06-16 03:02:05 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
update urdf2factory to spawn models given model name.
Modified Paths:
--------------
pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
Modified: pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-06-16 02:51:13 UTC (rev 17128)
+++ pkg/trunk/demos/door_demos_gazebo/launch/pr2_and_door.launch 2009-06-16 03:02:05 UTC (rev 17129)
@@ -1,33 +1,35 @@
<launch>
- <group name="gazebo">
- <param name="/use_sim_time" value="true" />
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find ogre)/ogre/lib:$(find opende)/opende/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
- <!-- send pr2.xml to param server -->
- <!-- push robotdesc/door to factory and spawn robot in gazebo -->
- <param ns="pr2" name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find door_demos_gazebo)/robots/pr2_and_door.xacro.xml'" />
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
-
- <!-- load door and handle controllers -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find door_demos_gazebo)/door_defs/door_controllers.xml" respawn="false" output="screen" />
- <node pkg="door_demos_gazebo" type="set_door.py" output="screen"/>
-
- <!-- uncomment for visualization -->
- <!--
- <node pkg="rviz" type="rviz" output="screen"/>
- -->
-
- <!-- Robot state publisher -->
- <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
- <param name="publish_frequency" type="double" value="50.0" />
- <param name="tf_prefix" type="string" value="" />
+ <group name="gazebo">
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find ogre)/ogre/lib:$(find opende)/opende/lib:$(optenv LD_LIBRARY_PATH)" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
</node>
-
- </group>
+
+ <!-- send pr2.xml to param server -->
+ <!-- push robotdesc/door to factory and spawn robot in gazebo -->
+ <param ns="pr2" name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
+
+ <param ns="door" name="robotdesc/door" command="$(find xacro)/xacro.py '$(find door_demos_gazebo)/door_defs/door_defs.xml'" />
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/door 0 0 0 0 0 0 doorbot" respawn="false" output="screen" />
+
+ <!-- load door and handle controllers -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find door_demos_gazebo)/door_defs/door_controllers.xml" respawn="false" output="screen" />
+ <node pkg="door_demos_gazebo" type="set_door.py" output="screen"/>
+
+ <!-- uncomment for visualization -->
+ <!--
+ <node pkg="rviz" type="rviz" output="screen"/>
+ -->
+
+ <!-- Robot state publisher -->
+ <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
+ <param name="publish_frequency" type="double" value="50.0" />
+ <param name="tf_prefix" type="string" value="" />
+ </node>
+
+ </group>
</launch>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-06-16 02:51:13 UTC (rev 17128)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-06-16 03:02:05 UTC (rev 17129)
@@ -51,8 +51,9 @@
void usage(const char *progname)
{
- printf("\nUsage: %s xml_param_name [initial x y z roll pitch yaw]\n", progname);
- printf(" e.g. read robotdesc/pr2 from param server and send to gazebo factory to spawn robot\n\n");
+ printf("\nUsage: %s xml_param_name [initial x y z roll pitch yaw gazebo_model_name]\n", progname);
+ printf(" For example: urdf2factory robot_descriptions 0 0 1 0 0 90 pr3_model\n\n");
+ printf(" Note: gazebo_model_name defaults to pr2_model.\n\n");
}
int main(int argc, char **argv)
@@ -84,9 +85,12 @@
std::string robot_model_name("pr2_model");
// make sure this is not the ros-generated commandline log filename
- if (argc >= 9 && robot_model_name.find(std::string(" __log:")) == robot_model_name.size())
+ if (argc >= 9)
{
- robot_model_name = std::string(argv[8]);
+ std::string name = std::string(argv[8]);
+ ROS_ERROR("Model Name: %s %d\n",name.c_str(),name.find(std::string(" __log:")));
+ if (name.find(std::string(" __log:")) == -1)
+ robot_model_name = name;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-06-16 07:39:34
|
Revision: 17139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17139&view=rev
Author: mariusmuja
Date: 2009-06-16 07:39:29 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
Added asterisk in 3rdparty.
Scripts for asterisk based navigation.
Added Paths:
-----------
pkg/trunk/3rdparty/asterisk/
pkg/trunk/3rdparty/asterisk/Makefile
pkg/trunk/3rdparty/asterisk/files/
pkg/trunk/3rdparty/asterisk/files/agi.py
pkg/trunk/3rdparty/asterisk/files/extensions.ael
pkg/trunk/3rdparty/asterisk/files/office.py
pkg/trunk/3rdparty/asterisk/manifest.xml
pkg/trunk/highlevel/executive_python/src/office_nav.py
pkg/trunk/highlevel/executive_python/src/offices.py
pkg/trunk/highlevel/executive_python/src/offices_outside.py
pkg/trunk/highlevel/executive_python/src/print_goal.py
pkg/trunk/highlevel/executive_python/src/send_to_office.py
Added: pkg/trunk/3rdparty/asterisk/Makefile
===================================================================
--- pkg/trunk/3rdparty/asterisk/Makefile (rev 0)
+++ pkg/trunk/3rdparty/asterisk/Makefile 2009-06-16 07:39:29 UTC (rev 17139)
@@ -0,0 +1,24 @@
+all: installed
+
+VERSION = 1.6.1.0
+TARBALL = build/asterisk-$(VERSION).tar.gz
+TARBALL_URL = http://downloads.asterisk.org/pub/telephony/asterisk/releases/asterisk-$(VERSION).tar.gz
+UNPACK_CMD = tar -xvzf
+SOURCE_DIR = build/asterisk-$(VERSION)
+
+include $(shell rospack find mk)/download_unpack_build.mk
+
+installed: wiped $(SOURCE_DIR)/unpacked
+ echo "Building Asterisk PBX..."
+ cd $(SOURCE_DIR) && ./configure --prefix=`pwd`/../asterisk && make && make install && make samples && rm main/asterisk # detele asterisk from source folder so that rosrun will not find it
+ cp files/extensions.ael build/asterisk/etc/asterisk
+ -cp files/sip.conf build/asterisk/etc/asterisk
+ cp files/*.py build/asterisk/var/lib/asterisk/agi-bin
+ touch installed
+
+wiped: Makefile
+ make wipe
+ touch wiped
+
+wipe:
+ rm -rf build installed
Added: pkg/trunk/3rdparty/asterisk/files/agi.py
===================================================================
--- pkg/trunk/3rdparty/asterisk/files/agi.py (rev 0)
+++ pkg/trunk/3rdparty/asterisk/files/agi.py 2009-06-16 07:39:29 UTC (rev 17139)
@@ -0,0 +1,665 @@
+#!/usr/bin/env python
+# vim: set et sw=4:
+"""agi
+
+This module contains functions and classes to implment AGI scripts in python.
+pyvr
+
+{'agi_callerid' : 'mars.putland.int',
+ 'agi_channel' : 'IAX[kputland@kputland]/119',
+ 'agi_context' : 'default',
+ 'agi_dnid' : '666',
+ 'agi_enhanced' : '0.0',
+ 'agi_extension': '666',
+ 'agi_language' : 'en',
+ 'agi_priority' : '1',
+ 'agi_rdnis' : '',
+ 'agi_request' : 'pyst',
+ 'agi_type' : 'IAX'}
+
+"""
+
+import sys, pprint, re
+from types import ListType
+import signal
+
+DEFAULT_TIMEOUT = 2000 # 2sec timeout used as default for functions that take timeouts
+DEFAULT_RECORD = 20000 # 20sec record time
+
+re_code = re.compile(r'(^\d*)\s*(.*)')
+re_kv = re.compile(r'(?P<key>\w+)=(?P<value>[^\s]+)\s*(?:\((?P<data>.*)\))*')
+
+class AGIException(Exception): pass
+class AGIError(AGIException): pass
+
+class AGIUnknownError(AGIError): pass
+
+class AGIAppError(AGIError): pass
+
+# there are several different types of hangups we can detect
+# they all are derrived from AGIHangup
+class AGIHangup(AGIAppError): pass
+class AGISIGHUPHangup(AGIHangup): pass
+class AGISIGPIPEHangup(AGIHangup): pass
+class AGIResultHangup(AGIHangup): pass
+
+class AGIDBError(AGIAppError): pass
+
+class AGIUsageError(AGIError): pass
+class AGIInvalidCommand(AGIError): pass
+
+class AGI:
+ """
+ This class encapsulates communication between Asterisk an a python script.
+ It handles encoding commands to Asterisk and parsing responses from
+ Asterisk.
+ """
+
+ def __init__(self):
+ self._got_sighup = False
+ signal.signal(signal.SIGHUP, self._handle_sighup) # handle SIGHUP
+ sys.stderr.write('ARGS: ')
+ sys.stderr.write(str(sys.argv))
+ sys.stderr.write('\n')
+ self.env = {}
+ self._get_agi_env()
+
+ def _get_agi_env(self):
+ while 1:
+ line = sys.stdin.readline().strip()
+ sys.stderr.write('ENV LINE: ')
+ sys.stderr.write(line)
+ sys.stderr.write('\n')
+ if line == '':
+ #blank line signals end
+ break
+ key,data = line.split(':')[0], ':'.join(line.split(':')[1:])
+ key = key.strip()
+ data = data.strip()
+ if key <> '':
+ self.env[key] = data
+ sys.stderr.write('class AGI: self.env = ')
+ sys.stderr.write(pprint.pformat(self.env))
+ sys.stderr.write('\n')
+
+ def _quote(self, string):
+ return ''.join(['"', str(string), '"'])
+
+ def _handle_sighup(self, signum, frame):
+ """Handle the SIGHUP signal"""
+ self._got_sighup = True
+
+ def test_hangup(self):
+ """This function throws AGIHangup if we have recieved a SIGHUP"""
+ if self._got_sighup:
+ raise AGISIGHUPHangup("Received SIGHUP from Asterisk")
+
+ def execute(self, command, *args):
+ self.test_hangup()
+
+ try:
+ self.send_command(command, *args)
+ return self.get_result()
+ except IOError,e:
+ if e.errno == 32:
+ # Broken Pipe * let us go
+ raise AGISIGPIPEHangup("Received SIGPIPE")
+ else:
+ raise
+
+ def send_command(self, command, *args):
+ """Send a command to Asterisk"""
+ command = command.strip()
+ command = '%s %s' % (command, ' '.join(map(str,args)))
+ command = command.strip()
+ if command[-1] != '\n':
+ command += '\n'
+ sys.stderr.write(' COMMAND: %s' % command)
+ sys.stdout.write(command)
+ sys.stdout.flush()
+
+ def get_result(self, stdin=sys.stdin):
+ """Read the result of a command from Asterisk"""
+ code = 0
+ result = {'result':('','')}
+ line = stdin.readline().strip()
+ sys.stderr.write(' RESULT_LINE: %s\n' % line)
+ m = re_code.search(line)
+ if m:
+ code, response = m.groups()
+ code = int(code)
+
+ if code == 200:
+ for key,value,data in re_kv.findall(response):
+ result[key] = (value, data)
+
+ # If user hangs up... we get 'hangup' in the data
+ if data == 'hangup':
+ raise AGIResultHangup("User hungup during execution")
+
+ if key == 'result' and value == '-1':
+ raise AGIAppError("Error executing application, or hangup")
+
+ sys.stderr.write(' RESULT_DICT: %s\n' % pprint.pformat(result))
+ return result
+ elif code == 510:
+ raise AGIInvalidCommand(response)
+ elif code == 520:
+ usage = [line]
+ line = stdin.readline().strip()
+ while line[:3] != '520':
+ usage.append(line)
+ line = stdin.readline().strip()
+ usage.append(line)
+ usage = '%s\n' % '\n'.join(usage)
+ raise AGIUsageError(usage)
+ else:
+ raise AGIUnknownError(code, 'Unhandled code or undefined response')
+
+ def _process_digit_list(self, digits):
+ if type(digits) == ListType:
+ digits = ''.join(map(str, digits))
+ return self._quote(digits)
+
+ def answer(self):
+ """agi.answer() --> None
+ Answer channel if not already in answer state.
+ """
+ self.execute('ANSWER')['result'][0]
+
+ def wait_for_digit(self, timeout=DEFAULT_TIMEOUT):
+ """agi.wait_for_digit(timeout=DEFAULT_TIMEOUT) --> digit
+ Waits for up to 'timeout' milliseconds for a channel to receive a DTMF
+ digit. Returns digit dialed
+ Throws AGIError on channel falure
+ """
+ res = self.execute('WAIT FOR DIGIT', timeout)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except ValueError:
+ raise AGIError('Unable to convert result to digit: %s' % res)
+
+ def send_text(self, text=''):
+ """agi.send_text(text='') --> None
+ Sends the given text on a channel. Most channels do not support the
+ transmission of text.
+ Throws AGIError on error/hangup
+ """
+ self.execute('SEND TEXT', self._quote(text))['result'][0]
+
+ def receive_char(self, timeout=DEFAULT_TIMEOUT):
+ """agi.receive_char(timeout=DEFAULT_TIMEOUT) --> chr
+ Receives a character of text on a channel. Specify timeout to be the
+ maximum time to wait for input in milliseconds, or 0 for infinite. Most channels
+ do not support the reception of text.
+ """
+ res = self.execute('RECEIVE CHAR', timeout)['result'][0]
+
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def tdd_mode(self, mode='off'):
+ """agi.tdd_mode(mode='on'|'off') --> None
+ Enable/Disable TDD transmission/reception on a channel.
+ Throws AGIAppError if channel is not TDD-capable.
+ """
+ res = self.execute('TDD MODE', mode)['result'][0]
+ if res == '0':
+ raise AGIAppError('Channel %s is not TDD-capable')
+
+ def stream_file(self, filename, escape_digits='', sample_offset=0):
+ """agi.stream_file(filename, escape_digits='', sample_offset=0) --> digit
+ Send the given file, allowing playback to be interrupted by the given
+ digits, if any. escape_digits is a string '12345' or a list of
+ ints [1,2,3,4,5] or strings ['1','2','3'] or mixed [1,'2',3,'4']
+ If sample offset is provided then the audio will seek to sample
+ offset before play starts. Returns digit if one was pressed.
+ Throws AGIError if the channel was disconnected. Remember, the file
+ extension must not be included in the filename.
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ response = self.execute('STREAM FILE', filename, escape_digits, sample_offset)
+ res = response['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def control_stream_file(self, filename, escape_digits='', skipms=3000, fwd='', rew='', pause=''):
+ """
+ Send the given file, allowing playback to be interrupted by the given
+ digits, if any. escape_digits is a string '12345' or a list of
+ ints [1,2,3,4,5] or strings ['1','2','3'] or mixed [1,'2',3,'4']
+ If sample offset is provided then the audio will seek to sample
+ offset before play starts. Returns digit if one was pressed.
+ Throws AGIError if the channel was disconnected. Remember, the file
+ extension must not be included in the filename.
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ response = self.execute('CONTROL STREAM FILE', self._quote(filename), escape_digits, self._quote(skipms), self._quote(fwd), self._quote(rew), self._quote(pause))
+ res = response['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def send_image(self, filename):
+ """agi.send_image(filename) --> None
+ Sends the given image on a channel. Most channels do not support the
+ transmission of images. Image names should not include extensions.
+ Throws AGIError on channel failure
+ """
+ res = self.execute('SEND IMAGE', filename)['result'][0]
+ if res != '0':
+ raise AGIAppError('Channel falure on channel %s' % self.env.get('agi_channel','UNKNOWN'))
+
+ def say_digits(self, digits, escape_digits=''):
+ """agi.say_digits(digits, escape_digits='') --> digit
+ Say a given digit string, returning early if any of the given DTMF digits
+ are received on the channel.
+ Throws AGIError on channel failure
+ """
+ digits = self._process_digit_list(digits)
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY DIGITS', digits, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_number(self, number, escape_digits=''):
+ """agi.say_number(number, escape_digits='') --> digit
+ Say a given digit string, returning early if any of the given DTMF digits
+ are received on the channel.
+ Throws AGIError on channel failure
+ """
+ number = self._process_digit_list(number)
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY NUMBER', number, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_alpha(self, characters, escape_digits=''):
+ """agi.say_alpha(string, escape_digits='') --> digit
+ Say a given character string, returning early if any of the given DTMF
+ digits are received on the channel.
+ Throws AGIError on channel failure
+ """
+ characters = self._process_digit_list(characters)
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY ALPHA', characters, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_phonetic(self, characters, escape_digits=''):
+ """agi.say_phonetic(string, escape_digits='') --> digit
+ Phonetically say a given character string, returning early if any of
+ the given DTMF digits are received on the channel.
+ Throws AGIError on channel failure
+ """
+ characters = self._process_digit_list(characters)
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY PHONETIC', characters, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_date(self, seconds, escape_digits=''):
+ """agi.say_date(seconds, escape_digits='') --> digit
+ Say a given date, returning early if any of the given DTMF digits are
+ pressed. The date should be in seconds since the UNIX Epoch (Jan 1, 1970 00:00:00)
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY DATE', seconds, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_time(self, seconds, escape_digits=''):
+ """agi.say_time(seconds, escape_digits='') --> digit
+ Say a given time, returning early if any of the given DTMF digits are
+ pressed. The time should be in seconds since the UNIX Epoch (Jan 1, 1970 00:00:00)
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('SAY TIME', seconds, escape_digits)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def say_datetime(self, seconds, escape_digits='', format='', zone=''):
+ """agi.say_datetime(seconds, escape_digits='', format='', zone='') --> digit
+ Say a given date in the format specfied (see voicemail.conf), returning
+ early if any of the given DTMF digits are pressed. The date should be
+ in seconds since the UNIX Epoch (Jan 1, 1970 00:00:00).
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ if format: format = self._quote(format)
+ res = self.execute('SAY DATETIME', seconds, escape_digits, format, zone)['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def get_data(self, filename, timeout=DEFAULT_TIMEOUT, max_digits=255):
+ """agi.get_data(filename, timeout=DEFAULT_TIMEOUT, max_digits=255) --> digits
+ Stream the given file and receive dialed digits
+ """
+ result = self.execute('GET DATA', filename, timeout, max_digits)
+ res, value = result['result']
+ return res
+
+ def get_option(self, filename, escape_digits='', timeout=0):
+ """agi.get_option(filename, escape_digits='', timeout=0) --> digit
+ Send the given file, allowing playback to be interrupted by the given
+ digits, if any. escape_digits is a string '12345' or a list of
+ ints [1,2,3,4,5] or strings ['1','2','3'] or mixed [1,'2',3,'4']
+ Returns digit if one was pressed.
+ Throws AGIError if the channel was disconnected. Remember, the file
+ extension must not be included in the filename.
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ if timeout:
+ response = self.execute('GET OPTION', filename, escape_digits, timeout)
+ else:
+ response = self.execute('GET OPTION', filename, escape_digits)
+
+ res = response['result'][0]
+ if res == '0':
+ return ''
+ else:
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to char: %s' % res)
+
+ def set_context(self, context):
+ """agi.set_context(context)
+ Sets the context for continuation upon exiting the application.
+ No error appears to be produced. Does not set exten or priority
+ Use at your own risk. Ensure that you specify a valid context.
+ """
+ self.execute('SET CONTEXT', context)
+
+ def set_extension(self, extension):
+ """agi.set_extension(extension)
+ Sets the extension for continuation upon exiting the application.
+ No error appears to be produced. Does not set context or priority
+ Use at your own risk. Ensure that you specify a valid extension.
+ """
+ self.execute('SET EXTENSION', extension)
+
+ def set_priority(self, priority):
+ """agi.set_priority(priority)
+ Sets the priority for continuation upon exiting the application.
+ No error appears to be produced. Does not set exten or context
+ Use at your own risk. Ensure that you specify a valid priority.
+ """
+ self.execute('set priority', priority)
+
+ def goto_on_exit(self, context='', extension='', priority=''):
+ context = context or self.env['agi_context']
+ extension = extension or self.env['agi_extension']
+ priority = priority or self.env['agi_priority']
+ self.set_context(context)
+ self.set_extension(extension)
+ self.set_priority(priority)
+
+ def record_file(self, filename, format='gsm', escape_digits='#', timeout=DEFAULT_RECORD, offset=0, beep='beep'):
+ """agi.record_file(filename, format, escape_digits, timeout=DEFAULT_TIMEOUT, offset=0, beep='beep') --> None
+ Record to a file until a given dtmf digit in the sequence is received
+ The format will specify what kind of file will be recorded. The timeout
+ is the maximum record time in milliseconds, or -1 for no timeout. Offset
+ samples is optional, and if provided will seek to the offset without
+ exceeding the end of the file
+ """
+ escape_digits = self._process_digit_list(escape_digits)
+ res = self.execute('RECORD FILE', self._quote(filename), format, escape_digits, timeout, offset, beep)['result'][0]
+ try:
+ return chr(int(res))
+ except:
+ raise AGIError('Unable to convert result to digit: %s' % res)
+
+ def set_autohangup(self, secs):
+ """agi.set_autohangup(secs) --> None
+ Cause the channel to automatically hangup at <time> seconds in the
+ future. Of course it can be hungup before then as well. Setting to
+ 0 will cause the autohangup feature to be disabled on this channel.
+ """
+ self.execute('SET AUTOHANGUP', time)
+
+ def hangup(self, channel=''):
+ """agi.hangup(channel='')
+ Hangs up the specified channel.
+ If no channel name is given, hangs up the current channel
+ """
+ self.execute('HANGUP', channel)
+
+ def appexec(self, application, options=''):
+ """agi.appexec(application, options='')
+ Executes <application> with given <options>.
+ Returns whatever the application returns, or -2 on failure to find
+ application
+ """
+ result = self.execute('EXEC', application, self._quote(options))
+ res = result['result'][0]
+ if res == '-2':
+ raise AGIAppError('Unable to find application: %s' % application)
+ return res
+
+ def set_callerid(self, number):
+ """agi.set_callerid(number) --> None
+ Changes the callerid of the current channel.
+ """
+ self.execute('SET CALLERID', number)
+
+ def channel_status(self, channel=''):
+ """agi.channel_status(channel='') --> int
+ Returns the status of the specified channel. If no channel name is
+ given the returns the status of the current channel.
+
+ Return values:
+ 0 Channel is down and available
+ 1 Channel is down, but reserved
+ 2 Channel is off hook
+ 3 Digits (or equivalent) have been dialed
+ 4 Line is ringing
+ 5 Remote end is ringing
+ 6 Line is up
+ 7 Line is busy
+ """
+ try:
+ result = self.execute('CHANNEL STATUS', channel)
+ except AGIHangup:
+ raise
+ except AGIAppError:
+ result = {'result': ('-1','')}
+
+ return int(result['result'][0])
+
+ def set_variable(self, name, value):
+ """Set a channel variable.
+ """
+ self.execute('SET VARIABLE', self._quote(name), self._quote(value))
+
+ def get_variable(self, name):
+ """Get a channel variable.
+
+ This function returns the value of the indicated channel variable. If
+ the variable is not set, an empty string is returned.
+ """
+ try:
+ result = self.execute('GET VARIABLE', self._quote(name))
+ except AGIResultHangup:
+ result = {'result': ('1', 'hangup')}
+
+ res, value = result['result']
+ return value
+
+ def get_full_variable(self, name, channel = None):
+ """Get a channel variable.
+
+ This function returns the value of the indicated channel variable. If
+ the variable is not set, an empty string is returned.
+ """
+ try:
+ if channel:
+ result = self.execute('GET FULL VARIABLE', self._quote(name), self._quote(channel))
+ else:
+ result = self.execute('GET FULL VARIABLE', self._quote(name))
+
+ except AGIResultHangup:
+ result = {'result': ('1', 'hangup')}
+
+ res, value = result['result']
+ return value
+
+ def verbose(self, message, level=1):
+ """agi.verbose(message='', level=1) --> None
+ Sends <message> to the console via verbose message system.
+ <level> is the the verbose level (1-4)
+ """
+ self.execute('VERBOSE', self._quote(message), level)
+
+ def database_get(self, family, key):
+ """agi.database_get(family, key) --> str
+ Retrieves an entry in the Asterisk database for a given family and key.
+ Returns 0 if <key> is not set. Returns 1 if <key>
+ is set and returns the variable in parenthesis
+ example return code: 200 result=1 (testvariable)
+ """
+ family = '"%s"' % family
+ key = '"%s"' % key
+ result = self.execute('DATABASE GET', self._quote(family), self._quote(key))
+ res, value = result['result']
+ if res == '0':
+ raise AGIDBError('Key not found in database: family=%s, key=%s' % (family, key))
+ elif res == '1':
+ return value
+ else:
+ raise AGIError('Unknown exception for : family=%s, key=%s, result=%s' % (family, key, pprint.pformat(result)))
+
+ def database_put(self, family, key, value):
+ """agi.database_put(family, key, value) --> None
+ Adds or updates an entry in the Asterisk database for a
+ given family, key, and value.
+ """
+ result = self.execute('DATABASE PUT', self._quote(family), self._quote(key), self._quote(value))
+ res, value = result['result']
+ if res == '0':
+ raise AGIDBError('Unable to put vaule in databale: family=%s, key=%s, value=%s' % (family, key, value))
+
+ def database_del(self, family, key):
+ """agi.database_del(family, key) --> None
+ Deletes an entry in the Asterisk database for a
+ given family and key.
+ """
+ result = self.execute('DATABASE DEL', self._quote(family), self._quote(key))
+ res, value = result['result']
+ if res == '0':
+ raise AGIDBError('Unable to delete from database: family=%s, key=%s' % (family, key))
+
+ def database_deltree(self, family, key=''):
+ """agi.database_deltree(family, key='') --> None
+ Deletes a family or specific keytree with in a family
+ in the Asterisk database.
+ """
+ result = self.execute('DATABASE DELTREE', self._quote(family), self._quote(key))
+ res, value = result['result']
+ if res == '0':
+ raise AGIDBError('Unable to delete tree from database: family=%s, key=%s' % (family, key))
+
+ def noop(self):
+ """agi.noop() --> None
+ Does nothing
+ """
+ self.execute('NOOP')
+
+if __name__=='__main__':
+ agi = AGI()
+ #agi.appexec('festival','Welcome to Klass Technologies. Thank you for calling.')
+ #agi.appexec('festival','This is a test of the text to speech engine.')
+ #agi.appexec('festival','Press 1 for sales ')
+ #agi.appexec('festival','Press 2 for customer support ')
+ #agi.hangup()
+ #agi.goto_on_exit(extension='1234', priority='1')
+ #sys.exit(0)
+ #agi.say_digits([4,5,6])
+ agi.say_number(agi.env["agi_extension"])
+ #agi.say_number('01234') # 668
+ #agi.say_number('0xf5') # 245
+ #agi.get_data('demo-congrats')
+ agi.hangup()
+ sys.exit(0)
+ #agi.record_file('pyst-test') #FAILS
+ #agi.stream_file('demo-congrats', [1,2,3,4,5,6,7,8,9,0,'#','*'])
+ #agi.appexec('background','demo-congrats')
+
+ try:
+ agi.appexec('backgrounder','demo-congrats')
+ except AGIAppError:
+ sys.stderr.write("Handled exception for missing application backgrounder\n")
+
+ agi.set_variable('foo','bar')
+ agi.get_variable('foo')
+
+ try:
+ agi.get_variable('foobar')
+ except AGIAppError:
+ sys.stderr.write("Handled exception for missing variable foobar\n")
+
+ try:
+ agi.database_put('foo', 'bar', 'foobar')
+ agi.database_put('foo', 'baz', 'foobaz')
+ agi.database_put('foo', 'bat', 'foobat')
+ v = agi.database_get('foo', 'bar')
+ sys.stderr.write('DBVALUE foo:bar = %s\n' % v)
+ v = agi.database_get('bar', 'foo')
+ sys.stderr.write('DBVALUE foo:bar = %s\n' % v)
+ agi.database_del('foo', 'bar')
+ agi.database_deltree('foo')
+ except AGIDBError:
+ sys.stderr.write("Handled exception for missing database entry bar:foo\n")
+
+ agi.hangup()
Added: pkg/trunk/3rdparty/asterisk/files/extensions.ael
===================================================================
--- pkg/trunk/3rdparty/asterisk/files/extensions.ael (rev 0)
+++ pkg/trunk/3rdparty/asterisk/files/extensions.ael 2009-06-16 07:39:29 UTC (rev 17139)
@@ -0,0 +1,471 @@
+//
+// Example AEL config file
+//
+//
+// Static extension configuration file, used by
+// the pbx_ael module. This is where you configure all your
+// inbound and outbound calls in Asterisk.
+//
+// This configuration file is reloaded
+// - With the "ael reload" command in the CLI
+// - With the "reload" command (that reloads everything) in the CLI
+
+// The "Globals" category contains global variables that can be referenced
+// in the dialplan by using the GLOBAL dialplan function:
+// ${GLOBAL(VARIABLE)}
+// ${${GLOBAL(VARIABLE)}} or ${text${GLOBAL(VARIABLE)}} or any hybrid
+// Unix/Linux environmental variables are reached with the ENV dialplan
+// function: ${ENV(VARIABLE)}
+//
+
+globals {
+ CONSOLE="Console/dsp"; // Console interface for demo
+ //CONSOLE=DAHDI/1
+ //CONSOLE=Phone/phone0
+ IAXINFO=guest; // IAXtel username/password
+ //IAXINFO="myuser:mypass";
+ TRUNK="DAHDI/G2"; // Trunk interface
+ ...
[truncated message content] |
|
From: <is...@us...> - 2009-06-16 18:53:48
|
Revision: 17169
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17169&view=rev
Author: isucan
Date: 2009-06-16 18:53:46 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
work on move_arm
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/CMakeLists.txt
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/move_arm.launch
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-16 18:53:46 UTC (rev 17169)
@@ -1,33 +1,9 @@
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(move_arm)
-#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()
-
rospack_add_executable(move_arm src/move_arm.cpp)
rospack_add_executable(move_arm_action_test test/test_move_arm.cpp)
-#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(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-06-16 18:53:46 UTC (rev 17169)
@@ -41,70 +41,79 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
+#include <robot_msgs/JointTraj.h>
#include <pr2_robot_actions/MoveArmState.h>
#include <pr2_robot_actions/MoveArmGoal.h>
+#include <motion_planning_msgs/KinematicPath.h>
-/** ROS */
#include <ros/ros.h>
-#include <vector>
-#include <string>
+#include <planning_environment/planning_monitor.h>
-/** IK services */
-#include <manipulation_srvs/IKService.h>
-#include <manipulation_srvs/IKQuery.h>
-
namespace move_arm
-{
- /**
- * @class MoveArm
- * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
- */
- class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
- {
- public:
+{
+
+ enum ArmType
+ {
+ LEFT, RIGHT
+ };
+
/**
- * @brief Constructor for the actions
+ * @class MoveArm
+ * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
*/
- MoveArm();
-
- /**
- * @brief Destructor - Cleans up
- */
- virtual ~MoveArm();
-
- /**
- * @brief Runs whenever a new goal is sent to the move_base
- * @param goal The goal to pursue
- * @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 pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback);
-
+ class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
+ {
+ public:
+ /**
+ * @brief Constructor for the actions
+ */
+ MoveArm(const ArmType &arm);
+
+ /**
+ * @brief Destructor - Cleans up
+ */
+ virtual ~MoveArm(void);
+
+ /**
+ * @brief Runs whenever a new goal is sent to the move_base
+ * @param goal The goal to pursue
+ * @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 pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback);
+
private:
+
+ // the state of the action; this should be true if initialized properly
+ bool valid_;
+
+ // the arm we are planning for
+ std::string arm_;
+ std::vector<std::string> arm_joint_names_;
- std::string ik_service_name_; /**< Name of the service that provides IK */
+
+ bool perform_ik_; /**< Flag that enables the option of IK */
+ std::string ik_service_name_; /**< Name of the service that provides IK */
+ std::string ik_query_name_; /**< Name of the service that allows you to query the joint names that IK uses */
- std::string ik_query_name_; /**< Name of the service that allows you to query the joint names that IK uses */
+ // service names
+ std::string motion_plan_name_;
+ std::string control_start_name_;
+ std::string control_query_name_;
+ std::string control_cancel_name_;
+
+ ros::NodeHandle node_handle_;
+ ros::Publisher displayPathPublisher_;
+
+ motion_planning_msgs::KinematicPath currentPath_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
- std::string control_query_name_;
+ void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj);
+ bool computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution);
+ bool getControlJointNames(std::vector<std::string> &joint_names);
+ };
+}
- std::string control_topic_name_;
-
- std::string control_service_name_;
-
- ros::NodeHandle node_handle_;
-
- bool computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<std::pair<std::string, double> > &solution);
-
- int arm_number_joints_;
-
- std::vector<std::string> arm_joint_names_;
-
- bool sendControl(const std::vector<std::pair<std::string, double> > &solution);
-
- bool getControlJointNames(std::vector<std::string> &joint_names);
- };
-};
-
#endif
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-06-16 18:53:46 UTC (rev 17169)
@@ -15,9 +15,9 @@
<depend package="robot_msgs"/>
<depend package="motion_planning_msgs"/>
<depend package="motion_planning_srvs"/>
+ <depend package="manipulation_srvs"/>
+ <depend package="planning_environment" />
+ <depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
- <depend package="robot_actions"/>
- <depend package="manipulation_srvs"/>
<depend package="pr2_mechanism_controllers"/>
-
</package>
Modified: pkg/trunk/highlevel/move_arm/move_arm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 18:53:46 UTC (rev 17169)
@@ -1,6 +1,6 @@
<launch>
- <node pkg="move_arm" type="move_arm" name="move_arm">
+ <node pkg="move_arm" type="move_arm" name="move_arm" output="screen">
<param name="ik_service_name" value="pr2_ik_node/ik_service" />
- <param name="ik_service" value="pr2_ik_node/ik_query" />
+ <param name="ik_query_name" value="pr2_ik_node/ik_query" />
</node>
-</launch>
\ No newline at end of file
+</launch>
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-16 18:53:46 UTC (rev 17169)
@@ -36,143 +36,401 @@
* Authors: Sachin Chitta, Ioan Sucan
*********************************************************************/
#include <move_arm/move_arm.h>
-#include <robot_msgs/JointTraj.h>
+
+#include <motion_planning_srvs/KinematicPlan.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <manipulation_srvs/IKService.h>
+#include <manipulation_srvs/IKQuery.h>
using namespace robot_actions;
namespace move_arm
{
- MoveArm::MoveArm() : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_arm")
- {
- node_handle_.param<int>("~arm_number_joints", arm_number_joints_,7);
- node_handle_.param<std::string>("~ik_service_name", ik_service_name_,"pr2_ik");
- node_handle_.param<std::string>("~ik_query_name", ik_query_name_, "pr2_query");
- node_handle_.param<std::string>("~control_topic_name",control_service_name_,"r_arm_joint_trajectory_controller/TrajectoryStart");
- node_handle_.param<std::string>("~control_query_name",control_query_name_,"r_arm_joint_trajectory_controller/TrajectoryQuery");
- }
+ MoveArm::MoveArm(const ArmType &arm) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_arm")
+ {
+ if (arm == LEFT)
+ arm_ = "left_arm";
+ else
+ arm_ = "right_arm";
+
+ ROS_INFO("Starting move_arm for '%s'", arm_.c_str());
+
+ // monitor robot
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
- MoveArm::~MoveArm()
- {
- }
-
- robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
- {
- robot_msgs::PoseStamped arm_cartesian_goal = goal.goal_constraints.pose_constraint[0].pose;
- std::vector<std::pair<std::string, double> > solution;
- if(!computeIK(arm_cartesian_goal,solution))
- {
- ROS_ERROR("MoveArm:: IK failed");
- return robot_actions::ABORTED;
+ node_handle_.param<bool> ("~perform_ik", perform_ik_, true);
+ node_handle_.param<std::string>("~ik_service_name", ik_service_name_, "pr2_ik");
+ node_handle_.param<std::string>("~ik_query_name", ik_query_name_, "pr2_ik_query");
+
+ node_handle_.param<std::string>("~motion_plan_name", motion_plan_name_, "plan_kinematic_path");
+ node_handle_.param<std::string>("~control_start_name", control_start_name_, "right_arm/trajectory_controller/TrajectoryStart");
+ node_handle_.param<std::string>("~control_query_name", control_query_name_, "right_arm/trajectory_controller/TrajectoryQuery");
+ node_handle_.param<std::string>("~control_cancel_name", control_cancel_name_, "right_arm/trajectory_controller/TrajectoryCancel");
+
+ if (!(valid_ = collisionModels_->loadedModels() && getControlJointNames(arm_joint_names_)))
+ ROS_ERROR("Move arm action is invalid");
+
+ // advertise the topic for displaying kinematic plans
+ displayPathPublisher_ = node_handle_.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 10);
}
- if(!sendControl(solution))
+
+ MoveArm::~MoveArm()
{
- ROS_ERROR("MoveArm:: Control failed");
- return robot_actions::ABORTED;
+ delete planningMonitor_;
+ delete collisionModels_;
}
-
- return robot_actions::SUCCESS;
- }
+
+ robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
+ {
+ if (!valid_)
+ {
+ ROS_ERROR("Move arm action has not been initialized properly");
+ return robot_actions::ABORTED;
+ }
+
+ motion_planning_srvs::KinematicPlan::Request req;
+ motion_planning_srvs::KinematicPlan::Response res;
+
+ req.params.model_id = arm_;
+ req.params.planner_id = "KPIECE";
+ req.params.distance_metric = "L2Square";
+
+ // this volume is only needed if planar or floating joints move in the space
+ req.params.volumeMin.x = req.params.volumeMin.y = req.params.volumeMin.z = 0.0;
+ req.params.volumeMax.x = req.params.volumeMax.y = req.params.volumeMax.z = 0.0;
+
+ req.goal_constraints = goal.goal_constraints;
+ req.path_constraints = goal.path_constraints;
+
+
+ if (perform_ik_ && // IK is enabled,
+ req.goal_constraints.joint_constraint.empty() && // we have no joint constraints on the goal,
+ req.goal_constraints.pose_constraint.size() == 1 && // we have a single pose constraint on the goal
+ req.goal_constraints.pose_constraint[0].type == motion_planning_msgs::PoseConstraint::POSITION_XYZ +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY && // that is active on all 6 DOFs
+ req.goal_constraints.pose_constraint[0].position_distance < 0.1 && // and the desired position and
+ req.goal_constraints.pose_constraint[0].orientation_distance < 0.1 && // orientation distances are small,
+ ((arm_ == "left_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back())
+ || (arm_ == "right_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back()))) // and acts on the last link of the arm
+ {
+ // we can do ik can turn the pose constraint into a joint one
+ ROS_INFO("Converting pose constraint to joint constraint using IK...");
+
+ std::vector<double> solution;
+ if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ {
+ req.goal_constraints.joint_constraint.resize(1);
+ req.goal_constraints.joint_constraint[0].header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
+ req.goal_constraints.joint_constraint[0].header.stamp = planningMonitor_->lastStateUpdate();
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ motion_planning_msgs::JointConstraint jc;
+ jc.joint_name = arm_joint_names_[i];
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ jc.value.push_back(solution[n + j]);
+ jc.toleranceAbove.push_back(0.0);
+ jc.toleranceBelow.push_back(0.0);
+ }
+ n += u;
+ req.goal_constraints.joint_constraint.push_back(jc);
+ }
+ req.goal_constraints.pose_constraint.clear();
+ }
+ else
+ ROS_WARN("Unable to compute IK");
+ }
+
+ req.times = 1;
+ req.allowed_time = 0.5;
+
+ ResultStatus result = robot_actions::SUCCESS;
+
+ feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ update(feedback);
+
+ ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_srvs::KinematicPlan>("plan_kinematic_path", true);
+ ros::ServiceClient clientStart = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>("right_arm/trajectory_controller/TrajectoryStart", true);
+ ros::ServiceClient clientQuery = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>("right_arm/trajectory_controller/TrajectoryQuery", true);
+ ros::ServiceClient clientCancel = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryCancel>("right_arm/trajectory_controller/TrajectoryCancel", true);
- bool MoveArm::sendControl(const std::vector<std::pair<std::string, double> > &solution)
- {
- robot_msgs::JointTraj joint_traj;
- joint_traj.set_points_size(1);
- joint_traj.points[0].set_positions_size(solution.size());
+ int trajectoryId = -1;
+ ros::Duration eps(0.01);
+ while (true)
+ {
+ // if we have to stop, do so
+ if (isPreemptRequested() || !node_handle_.ok())
+ result = robot_actions::PREEMPTED;
+
+ // if we have to plan, do so
+ if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
+ {
+ // call the planner and decide whether to use the path
+ if (clientPlan.call(req, res))
+ {
+ if (res.path.states.empty())
+ ROS_WARN("Unable to plan path to desired goal");
+ else
+ {
+ if (res.unsafe)
+ ROS_WARN("Received path is unsafe (planning data was out of date). Ignoring");
+ else
+ {
+ if (res.path.model_id != req.params.model_id)
+ ROS_ERROR("Received path for incorrect model: expected '%s', received '%s'", req.params.model_id.c_str(), res.path.model_id.c_str());
+ else
+ {
+ if (!planningMonitor_->getTransformListener()->frameExists(res.path.header.frame_id))
+ ROS_ERROR("Received path in unknown frame: '%s'", res.path.header.frame_id.c_str());
+ else
+ {
+ if (res.approximate)
+ ROS_INFO("Approximate path was found. Distance to goal is: %f", res.distance);
+ currentPath_ = res.path;
+ feedback = pr2_robot_actions::MoveArmState::MOVING;
+ update(feedback);
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ ROS_ERROR("Service 'plan_kinematic_path' failed");
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+
+ // if we have to stop, do so
+ if (isPreemptRequested() || !node_handle_.ok())
+ result = robot_actions::PREEMPTED;
- for(int j =0; j < (int) solution.size(); j++)
- {
- joint_traj.points[0].positions[j] = solution[j].second;
- }
- ros::ServiceClient client = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>(control_service_name_);
- pr2_mechanism_controllers::TrajectoryStart::Request req;
- pr2_mechanism_controllers::TrajectoryStart::Response res;
- req.traj = joint_traj;
- req.requesttiming = 1;
+ // stop the robot if we need to
+ if (feedback == pr2_robot_actions::MoveArmState::MOVING)
+ {
+ if (result == robot_actions::PREEMPTED || !planningMonitor_->isPathValid(currentPath_))
+ {
+ if (result == robot_actions::PREEMPTED)
+ ROS_INFO("Preempt requested. Stopping arm.");
+ else
+ ROS_INFO("Current path is no longer valid. Stopping & replanning...");
- ROS_DEBUG("MoveArm:: Sending out control");
- if(!client.call(req,res))
- return false;
- return true;
- }
+ if (trajectoryId != -1)
+ {
+ // we are already executing the path; we need to stop it
+ pr2_mechanism_controllers::TrajectoryCancel::Request send_traj_cancel_req;
+ pr2_mechanism_controllers::TrajectoryCancel::Response send_traj_cancel_res;
+ send_traj_cancel_req.trajectoryid = trajectoryId;
+ if (clientCancel.call(send_traj_cancel_req, send_traj_cancel_res))
+ {
+ // check if indeed the trajectory was canceled
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+ send_traj_query_req.trajectoryid = trajectoryId;
+ if (clientQuery.call(send_traj_query_req, send_traj_query_res))
+ {
+ if (send_traj_query_res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Active)
+ ROS_WARN("Unable to confirm canceling trajectory %d. Continuing...", trajectoryId);
+ }
+ else
+ ROS_ERROR("Unable to query trajectory %d. Continuing...", trajectoryId);
+ }
+ else
+ ROS_ERROR("Unable to cancel trajectory %d. Continuing...", trajectoryId);
+ trajectoryId = -1;
+ }
+
+ if (result != robot_actions::PREEMPTED)
+ {
+ // if we were not preempted
+ feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ update(feedback);
+ continue;
+ }
+ else
+ break;
+ }
+ }
+
+ // execute & monitor a path if we need to
+ if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::MOVING)
+ {
+ // start the controller if we have to, using trajectory start
+ if (trajectoryId == -1)
+ {
+ pr2_mechanism_controllers::TrajectoryStart::Request send_traj_start_req;
+ pr2_mechanism_controllers::TrajectoryStart::Response send_traj_start_res;
+
+ fillTrajectoryPath(currentPath_, send_traj_start_req.traj);
+ send_traj_start_req.hastiming = 0;
+ send_traj_start_req.requesttiming = 0;
- bool MoveArm::getControlJointNames(std::vector<std::string> &joint_names)
- {
- ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(control_query_name_);
- pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
- pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
- req_query.trajectoryid = -1;
+ if (clientStart.call(send_traj_start_req, send_traj_start_res))
+ {
+ trajectoryId = send_traj_start_res.trajectoryid;
+ if (trajectoryId < 0)
+ {
+ ROS_ERROR("Invalid trajectory id: %d", trajectoryId);
+ result = robot_actions::ABORTED;
+ break;
+ }
+ ROS_INFO("Sent trajectory to controller");
+ }
+ else
+ {
+ ROS_ERROR("Service 'right_arm/trajectory_controller/TrajectoryStart' failed");
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+
+ // monitor controller execution by calling trajectory query
- if(!client_query.call(req_query,res_query))
- return false;
- joint_names.resize(res_query.get_jointnames_size());
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+ send_traj_query_req.trajectoryid = trajectoryId;
+ if (clientQuery.call(send_traj_query_req, send_traj_query_res))
+ {
+ // we are done; exit with success
+ if (send_traj_query_res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Done)
+ break;
+ // something bad happened in the execution
+ if (send_traj_query_res.done != pr2_mechanism_controllers::TrajectoryQuery::Response::State_Active &&
+ send_traj_query_res.done != pr2_mechanism_controllers::TrajectoryQuery::Response::State_Queued)
+ {
+ result = robot_actions::ABORTED;
+ ROS_ERROR("Unable to execute trajectory %d: query returned status %d", trajectoryId, (int)send_traj_query_res.done);
+ break;
+ }
+ }
+ else
+ {
+ ROS_ERROR("Unable to query trajectory %d", trajectoryId);
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+ eps.sleep();
+ }
- for(int i=0; i < (int)res_query.get_jointnames_size(); i++)
- {
- joint_names[i] = res_query.jointnames[i];
- }
- return true;
- }
+ return result;
- bool MoveArm::computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<std::pair<std::string, double> > &solution)
- {
- // define the service messages
- manipulation_srvs::IKService::Request request;
- manipulation_srvs::IKService::Response response;
+ /*
- request.data.pose_stamped = pose_stamped_msg;
- request.data.set_positions_size(arm_number_joints_);
- request.data.set_joint_names_size(arm_number_joints_);
+
+ return robot_actions::SUCCESS;
- for(int i=0; i < arm_number_joints_; i++)
+ */
+ }
+
+ void MoveArm::fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
{
- request.data.positions[i] = 0.0; // TODO - FILL THIS UP
- }
- // BIG TODO - FILL THESE UP
- request.data.joint_names[0] = "r_shoulder_pan_joint";
- request.data.joint_names[1] = "r_shoulder_lift_joint";
- request.data.joint_names[2] = "r_upper_arm_roll_joint";
-
- request.data.joint_names[3] = "r_elbow_flex_joint";
- request.data.joint_names[4] = "r_forearm_roll_joint";
- request.data.joint_names[5] = "r_wrist_flex_joint";
- request.data.joint_names[6] = "r_wrist_roll_joint";
-
- ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ik_service_name_);
-
- if (client.call(request, response))
- {
- ROS_DEBUG("MoveArm:: Got IK solution");
- solution.resize(arm_number_joints_);
- for(int i=0; i< arm_number_joints_; i++)
- {
- solution[i].first = request.data.joint_names[i];
- solution[i].second = response.solution[i];
- ROS_DEBUG("Joint %s: %f",solution[i].first.c_str(),solution[i].second);
- }
- return true;
+ traj.points.resize(path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ traj.points[i].positions = path.states[i].vals;
+ traj.points[i].time = path.times[i];
+ }
}
- else
+
+ bool MoveArm::getControlJointNames(std::vector<std::string> &joint_names)
{
- ROS_ERROR("MoveArm:: Service %s failed",ik_service_name_.c_str());
- return false;
+ ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(control_query_name_);
+ pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
+ pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
+ req_query.trajectoryid = -1;
+
+ if (!client_query.call(req_query, res_query))
+ {
+ ROS_ERROR("Unable to retrieve controller joint names using '%s' service", control_query_name_.c_str());
+ return false;
+ }
+
+ joint_names = res_query.jointnames;
+
+ // make sure we have the right joint names
+ for(unsigned int i = 0; i < joint_names.size() ; ++i)
+ {
+ if (planning_models::KinematicModel::Joint *j = planningMonitor_->getKinematicModel()->getJoint(joint_names[i]))
+ {
+ ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
+ if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, arm_) < 0)
+ return false;
+ }
+ else
+ {
+ ROS_ERROR("Joint '%s' is not known", joint_names[i].c_str());
+ return false;
+ }
+ }
+
+ std::vector<std::string> groupNames;
+ planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, arm_);
+ if (groupNames.size() != joint_names.size())
+ {
+ ROS_ERROR("The group '%s' has more joints than the controller can handle", arm_.c_str());
+ return false;
+ }
+
+ if (groupNames != joint_names)
+ ROS_WARN("The group joints are not in the same order as the controller expects");
+
+ return true;
}
- }
-};
+
+ bool MoveArm::computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution)
+ {
+ // define the service messages
+ manipulation_srvs::IKService::Request request;
+ manipulation_srvs::IKService::Response response;
+
+ request.data.pose_stamped = pose_stamped_msg;
+ request.data.joint_names = arm_joint_names_;
+ request.data.positions.clear();
+ for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
+ {
+ const double *params = planningMonitor_->getRobotState()->getParamsJoint(arm_joint_names_[i]);
+ const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ request.data.positions.push_back(params[j]);
+ }
+
+ ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ik_service_name_);
+
+ if (client.call(request, response))
+ {
+ ROS_DEBUG("MoveArm:: Got IK solution");
+ solution = response.solution;
+ for(unsigned int i = 0; i < solution.size() ; ++i)
+ ROS_DEBUG("%f", solution[i]);
+ return true;
+ }
+ else
+ {
+ ROS_ERROR("MoveArm:: Service '%s' failed", ik_service_name_.c_str());
+ return false;
+ }
+ }
+
+}
-int main(int argc, char** argv){
-// ros::init(argc, argv, "move_arm");
- ros::init(argc,argv);
- ros::Node ros_node("move_arm");
-
- move_arm::MoveArm move_arm;
- robot_actions::ActionRunner runner(20.0);
- runner.connect<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t>(move_arm);
- runner.run();
- ros::spin();
- return(0);
-
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "move_arm");
+ ros::Node xx; // hack to get action to work
+
+ move_arm::MoveArm move_arm(move_arm::RIGHT);
+ robot_actions::ActionRunner runner(20.0);
+ runner.connect<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t>(move_arm);
+ runner.run();
+ ros::spin();
+ return 0;
}
Modified: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-16 18:53:46 UTC (rev 17169)
@@ -51,44 +51,44 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv);
- ros::Node node("test_move_arm");
-// boost::thread* thread;
-
- pr2_robot_actions::SwitchControllers switchlist;
- std_msgs::Empty empty;
-
- Duration timeout_short = Duration().fromSec(2.0);
- Duration timeout_medium = Duration().fromSec(10.0);
- Duration timeout_long = Duration().fromSec(40.0);
-
-// robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
-
- int32_t feedback;
- pr2_robot_actions::MoveArmGoal goal;
- pr2_robot_actions::MoveArmState state;
-
- goal.goal_constraints.set_pose_constraint_size(1);
- goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
- goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
-
- goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
-
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
- goal.goal_constraints.pose_constraint[0].p...
[truncated message content] |
|
From: <is...@us...> - 2009-06-16 23:45:30
|
Revision: 17182
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17182&view=rev
Author: isucan
Date: 2009-06-16 23:45:28 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
some launch files
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/manifest.xml
Added Paths:
-----------
pkg/trunk/demos/tabletop_manipulation/launch/old/
pkg/trunk/demos/tabletop_manipulation/launch/old/arm.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/base.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/controllers.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/controllers_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/debug_plot.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/ekf.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/full_body.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/gazebo.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/head_trajectory_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/old/hw.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/l_gripper_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/old/laser.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/laser_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/nav.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/nav_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/perception.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/perception_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/planning.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/r_gripper_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/old/sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/teleop.launch
pkg/trunk/demos/tabletop_manipulation/launch/old/torso_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/old/trajectory_controllers.xml
pkg/trunk/demos/tabletop_manipulation/launch/old/two_arm_trajectory_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/perception.launch
pkg/trunk/demos/tabletop_manipulation/launch/simulation.launch
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/point_cloud_assembler.launch
pkg/trunk/mapping/collision_map/collision_map.launch
Removed Paths:
-------------
pkg/trunk/demos/tabletop_manipulation/launch/arm.launch
pkg/trunk/demos/tabletop_manipulation/launch/base.launch
pkg/trunk/demos/tabletop_manipulation/launch/controllers.launch
pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/debug_plot.launch
pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch
pkg/trunk/demos/tabletop_manipulation/launch/full_body.launch
pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
pkg/trunk/demos/tabletop_manipulation/launch/head_trajectory_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/hw.launch
pkg/trunk/demos/tabletop_manipulation/launch/l_gripper_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/laser.launch
pkg/trunk/demos/tabletop_manipulation/launch/laser_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/perception.launch
pkg/trunk/demos/tabletop_manipulation/launch/perception_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/planning.launch
pkg/trunk/demos/tabletop_manipulation/launch/r_gripper_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
pkg/trunk/demos/tabletop_manipulation/launch/torso_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/trajectory_controllers.xml
pkg/trunk/demos/tabletop_manipulation/launch/two_arm_trajectory_controller.xml
pkg/trunk/mapping/collision_map/collision_map.xml
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/arm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/arm.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/arm.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,37 +0,0 @@
-<launch>
-<!-- Arm trajectory controller -->
- <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
-
- <param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.1"/>
-
- <param name="left_arm_trajectory_controller/velocity_scaling_factor" type="double" value="1.0"/>
- <param name="left_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
-
- <param name="left_arm_trajectory_controller/l_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="left_arm_trajectory_controller/l_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.1"/>
-
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/trajectory_controllers.xml" output="screen"/>
-
- <!-- Gripper effort controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/r_gripper_effort_controller.xml" output="screen"/>
-
- <!-- Torso effort controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/torso_effort_controller.xml" output="screen"/>
-
-</launch>
-
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/base.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/base.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/base.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,4 +0,0 @@
-<launch>
-<param name="base_controller/odom_publish_rate" value="30.0"/>
-<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/base_controller.xml" output="screen"/>
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/controllers.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/controllers.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/controllers.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,5 +0,0 @@
-<launch>
- <include file="$(find tabletop_manipulation)/launch/laser.launch"/>
- <include file="$(find tabletop_manipulation)/launch/base.launch"/>
- <!--include file="$(find tabletop_manipulation)/launch/arm.launch"/-->
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,7 +0,0 @@
-<launch>
- <include file="$(find tabletop_manipulation)/launch/laser_sim.launch"/>
- <include file="$(find tabletop_manipulation)/launch/arm.launch"/>
- <include file="$(find tabletop_manipulation)/launch/base.launch"/>
-
- <node pkg="tabletop_manipulation" type="tuckarm.py" args="left"/>
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/debug_plot.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/debug_plot.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/debug_plot.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,5 +0,0 @@
-<launch>
- <node pkg="rosviz" type="rxplot" args="-m. -p10 /mechanism_state/joint_states[25]/position:commanded_effort,/mechanism_state/joint_states[28]/position:commanded_effort" output="screen"/>
- <node pkg="rosviz" type="rxplot" args="-m. -p10 /mechanism_state/joint_states[25]/position,/mechanism_state/joint_states[28]/position" output="screen"/>
-</launch>
-
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,12 +0,0 @@
-<launch>
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
- <param name="robot_pose_ekf/freq" value="30.0"/>
- <param name="robot_pose_ekf/sensor_timeout" value="1.0"/>
- <param name="robot_pose_ekf/odom_used" value="true"/>
- <param name="robot_pose_ekf/imu_used" value="true"/>
- <param name="robot_pose_ekf/vo_used" value="false"/>
-
- <node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"
- machine="four"/>
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/full_body.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/full_body.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/full_body.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,36 +0,0 @@
-<launch>
-<!-- Arm trajectory controller -->
- <param name="arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
- <param name="arm_trajectory_controller/trajectory_wait_timeout" type="double" value="0.25"/>
-
- <param name="arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_upper_arm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
-
- <param name="arm_trajectory_controller/l_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
- <param name="arm_trajectory_controller/l_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/two_arm_trajectory_controller.xml" output="screen"/>
-
- <!-- Gripper effort controller -->
-
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/r_gripper_effort_controller.xml" output="screen"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/l_gripper_effort_controller.xml" output="screen"/>
-
- <!-- Torso effort controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/torso_effort_controller.xml" output="screen"/>
-
- <!-- Head trajectory controller -->
- <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/head_trajectory_controller.xml" output="screen"/>
-
-</launch>
-
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,17 +0,0 @@
-<launch>
- <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/pr2_defs/robots for full pr2 -->
- <!-- send pr2.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
-
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find tabletop_manipulation)/gazebo/simple.world" respawn="false">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description):$(find gazebo)/gazebo/share/gazebo" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- </node>
-
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0.5 0 0 0 0 0" respawn="false" output="screen" />
-
-</launch>
-
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/head_trajectory_controller.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/head_trajectory_controller.xml 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/head_trajectory_controller.xml 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
- <controller name="head/trajectory_controller" type="ArmTrajectoryControllerNode">
- <listen_topic name="head/trajectory_controller/trajectory_command" />
- <kinematics>
- <elem key="kdl_chain_name">head</elem>
- </kinematics>
- <map name="controller_param">
- <elem key="kdl_chain_name">head</elem>
- </map>
- <controller name="head_pan_controller" topic="head_pan_controller" type="JointPDController">
- <joint name="head_pan_joint" >
- <pid p="5.0" d="0.5" i="12.0" iClamp="0.5" />
- </joint>
- </controller>
- <controller name="head_tilt_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
- <joint name="head_tilt_joint" >
- <pid p="8.0" d="0.5" i="2.0" iClamp="0.1" />
- </joint>
- </controller>
- <trajectory interpolation="cubic" />
- </controller>
-</controllers>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/hw.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/hw.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/hw.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,20 +0,0 @@
-<launch>
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
- <!--include file="$(find tabletop_manipulation)/launch/teleop.launch"/-->
-
- <!-- We're working without a map, relying on IMU-corrected wheel odom -->
- <param name="/global_frame_id" value="odom_combined_offset"/>
- <node pkg="tf" type="transform_sender" args="-20 -20 0 0 0 0 odom_combined_offset odom_combined 10"/>
-
- <!-- Everything depends on ekf.launch, because it defines the
- odom_combined frame -->
- <include file="$(find tabletop_manipulation)/launch/ekf.launch"/>
-
- <include file="$(find tabletop_manipulation)/launch/controllers.launch"/>
- <include file="$(find tabletop_manipulation)/launch/perception.launch"/>
- <!-- Note that nav.launch requires perception.launch, because the latter
- runs the filters on the tilt laser scans -->
- <include file="$(find tabletop_manipulation)/launch/nav.launch"/>
- <!--include file="$(find tabletop_manipulation)/launch/planning.launch"/-->
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/l_gripper_effort_controller.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/l_gripper_effort_controller.xml 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/l_gripper_effort_controller.xml 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,8 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
- <controller name="left_gripper/effort_controller" type="JointEffortControllerNode">
- <joint name="l_gripper_l_finger_joint" />
- </controller>
-</controllers>
-
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/laser.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,4 +0,0 @@
-<launch>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" output="screen"/>
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 0.78 0.3" />
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/laser_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/laser_sim.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/laser_sim.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,4 +0,0 @@
-<launch>
- <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_default_controllers)/laser_tilt_controller.xml" output="screen"/>
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 4 0.78 0.3" />
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,69 +0,0 @@
-<launch>
- <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-
- <node pkg="map_server" type="map_server" args="$(find old_2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
-
-
- <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen" machine="three">
- <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
-
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="30.0"/>
- <param name="move_base/map_update_frequency" value="2.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="0.5"/>
- <param name="costmap_2d/base_laser_max_range" value="20.0"/>
- <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
- <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
- <param name="costmap_2d/z_threshold_max" value="2.0"/>
- <param name="costmap_2d/z_threshold_min" value="0.13"/>
- <param name="costmap_2d/no_information_value" value="255"/>
- <param name="costmap_2d/freespace_projection_height" value="2.0"/>
- <param name="costmap_2d/inflation_radius" value="0.65"/>
- <param name="costmap_2d/circumscribed_radius" value="0.46"/>
- <param name="costmap_2d/inscribed_radius" value="0.325"/>
- <param name="costmap_2d/raytrace_window" value="10.0"/>
- <param name="costmap_2d/weight" value="0.1"/>
-
-
- <param name="trajectory_rollout/map_size" value="4.0" />
- <param name="trajectory_rollout/acc_limit_x" value="2.5" />
- <param name="trajectory_rollout/acc_limit_y" value="2.5" />
- <param name="trajectory_rollout/acc_limit_th" value="3.2" />
- <param name="trajectory_rollout/sim_time" value="1.0" />
- <param name="trajectory_rollout/sim_granularity" value="0.05" />
- <param name="trajectory_rollout/vx_samples" value="15" />
- <param name="trajectory_rollout/vtheta_samples" value="15" />
- <param name="trajectory_rollout/path_distance_bias" value="0.6" />
- <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
- <param name="trajectory_rollout/occdist_scale" value="0.2" />
- <param name="trajectory_rollout/heading_lookahead" value="0.325" />
- <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
- <param name="trajectory_rollout/holonomic_robot" value="true" />
- <param name="trajectory_rollout/max_vel_x" value="0.5" />
- <param name="trajectory_rollout/min_vel_x" value="0.1" />
- <param name="trajectory_rollout/max_vel_th" value="1.0" />
- <param name="trajectory_rollout/min_vel_th" value="-1.0" />
- <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
- <param name="trajectory_rollout/freespace_model" value="true" />
- <param name="trajectory_rollout/dwa" value="false" />
- <param name="trajectory_rollout/simple_attractor" value="false" />
-
- <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
- <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
-
- <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
- way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
- <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
-
- <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
- are not getting any data -->
- <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
- <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
- <param name="costmap_2d/stereo_update_rate" value="0.0"/>
- </node>
-
-</launch>
Deleted: pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-06-16 23:37:08 UTC (rev 17181)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -1,71 +0,0 @@
-<launch>
-
- <node pkg="map_server" type="map_server" args="$(find old_2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
-
-
- <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen">
- <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
-
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="30.0"/>
- <param name="move_base/map_update_frequency" value="2.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="0.5"/>
- <param name="move_base/trans_stopped_velocity" value="0.01"/>
- <param name="move_base/rot_stopped_velocity" value="0.01"/>
-
- <param name="costmap_2d/base_laser_max_range" value="20.0"/>
- <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
- <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
- <param name="costmap_2d/z_threshold_max" value="2.0"/>
- <param name="costmap_2d/z_threshold_min" value="0.13"/>
- <param name="costmap_2d/no_information_value" value="255"/>
- <param name="costmap_2d/freespace_projection_height" value="2.0"/>
- <param name="costmap_2d/inflation_radius" value="0.65"/>
- <param name="costmap_2d/circumscribed_radius" value="0.46"/>
- <param name="costmap_2d/inscribed_radius" value="0.325"/>
- <param name="costmap_2d/raytrace_window" value="10.0"/>
- <param name="costmap_2d/weight" value="0.1"/>
-
-
- <param name="trajectory_rollout/map_size" value="4.0" />
- <param name="trajectory_rollout/acc_limit_x" value="2.5" />
- <param name="trajectory_rollout/acc_limit_y" value="2.5" />
- <param name="trajectory_rollout/acc_limit_th" value="3.2" />
- <param name="trajectory_rollout/sim_time" value="1.0" />
- <param name="trajectory_rollout/sim_granularity" value="0.05" />
- <param name="trajectory_rollout/vx_samples" value="15" />
- <param name="trajectory_rollout/vtheta_samples" value="15" />
- <param name="trajectory_rollout/path_distance_bias" value="0.6" />
- <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
- <param name="trajectory_rollout/occdist_scale" value="0.2" />
- <param name="trajectory_rollout/heading_lookahead" value="0.325" />
- <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
- <param name="trajectory_rollout/holonomic_robot" value="true" />
- <param name="trajectory_rollout/max_vel_x" value="0.5" />
- <param name="trajectory_rollout/min_vel_x" value="0.1" />
- <param name="trajectory_rollout/max_vel_th" value="1.0" />
- <param name="trajectory_rollout/min_vel_th" value="-1.0" />
- <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
- <param name="trajectory_rollout/freespace_model" value="true" />
- <param name="trajectory_rollout/dwa" value="false" />
- <param name="trajectory_rollout/simple_attractor" value="false" />
-
- <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
- <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
-
- <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
- way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
- <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
-
- <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
- are not getting any data -->
- <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
- <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
- <param name="costmap_2d/stereo_update_rate" value="0.0"/>
- </node>
-
-</launch>
Copied: pkg/trunk/demos/tabletop_manipulation/launch/old/arm.launch (from rev 17180, pkg/trunk/demos/tabletop_manipulation/launch/arm.launch)
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/old/arm.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/old/arm.launch 2009-06-16 23:45:28 UTC (rev 17182)
@@ -0,0 +1,37 @@
+<launch>
+<!-- Arm trajectory controller -->
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
+
+ <param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.1"/>
+
+ <param name="left_arm_trajectory_controller/velocity_scaling_factor" type="double" value="1.0"/>
+ <param name="left_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
+
+ <param name="left_arm_trajectory_controller/l_should...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X