Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2009-03-13 03:06:13
|
Revision: 12486
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12486&view=rev
Author: tfoote
Date: 2009-03-13 01:58:34 +0000 (Fri, 13 Mar 2009)
Log Message:
-----------
removing dependency on rosrecord for runtime usages only, for it is a core dependency and is already built in the bootstrap
Modified Paths:
--------------
pkg/trunk/calibration/laser_camera_calibration/manifest.xml
pkg/trunk/calibration/optical_flag_calibration/manifest.xml
pkg/trunk/demos/2dnav_pr2/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/estimation/robot_pose_ekf/manifest.xml
pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/gripper_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/head_test/head_impact_test/manifest.xml
pkg/trunk/hardware_test/life_test/head_test/head_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_impact_test/manifest.xml
pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/hot_box_test/manifest.xml
pkg/trunk/hardware_test/life_test/torso_lift_test/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/head_test_cart/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_arm/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_full/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_pan_tilt/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/manifest.xml
Modified: pkg/trunk/calibration/laser_camera_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -12,7 +12,6 @@
<depend package="robot_msgs"/>
<depend package="std_msgs"/>
<depend package="laser_scan"/>
- <depend package="rosrecord"/>
<depend package="rospy"/>
<depend package="rosoct"/>
<depend package="openraveros"/>
Modified: pkg/trunk/calibration/optical_flag_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/calibration/optical_flag_calibration/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -13,9 +13,6 @@
<!-- depend package="newmat10"/ -->
<depend package="pr2_mechanism_controllers" />
- # For Scripts
- <depend package="rosrecord" />
-
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
</export>
Modified: pkg/trunk/demos/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/demos/2dnav_pr2/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,7 +8,6 @@
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="robot_pose_ekf"/>
- <depend package="rosrecord" />
<depend package="map_server"/>
<depend package="executive_trex_pr2"/>
<depend package="trajectory_rollout"/>
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -16,6 +16,5 @@
# For Testing
<depend package="rostest"/>
<depend package="gtest"/>
- <depend package="rosrecord" />
</package>
Modified: pkg/trunk/estimation/robot_pose_ekf/manifest.xml
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/estimation/robot_pose_ekf/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,5 +8,4 @@
<depend package="robot_msgs" />
<depend package="tf" />
<depend package="gtest"/>
-<depend package="rosrecord"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -9,7 +9,6 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
<depend package="rospy"/>
- <depend package="rosrecord"/>
<depend package="spacenav_node"/>
<depend package="std_msgs"/>
<depend package="teleop_spacenav"/>
Modified: pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -9,7 +9,6 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
<depend package="pr2_alpha"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,6 +8,5 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/gripper_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/gripper_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/gripper_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,7 +8,6 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
- <depend package="rosrecord"/>
<depend package="robot_mechanism_controllers"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/head_test/head_impact_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/head_test/head_impact_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/head_test/head_impact_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,7 +8,6 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
- <depend package="rosrecord"/>
<depend package="pr2_mechanism_controllers"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/head_test/head_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/head_test/head_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/head_test/head_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -14,6 +14,5 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_impact_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_impact_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_impact_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -8,7 +8,6 @@
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
- <depend package="rosrecord"/>
<depend package="pr2_mechanism_controllers"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_life_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/hokuyo_test/hokuyo_life_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -14,5 +14,4 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/hardware_test/life_test/hot_box_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/hot_box_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/hot_box_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -22,6 +22,5 @@
<depend package="arm_life_test" />
<depend package="torso_lift_test" />
<depend package="pr2_mechanism_controllers"/>
- <depend package="rosrecord"/>
<depend package="ocean_battery_driver" />
</package>
Modified: pkg/trunk/hardware_test/life_test/torso_lift_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/torso_lift_test/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/hardware_test/life_test/torso_lift_test/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -14,5 +14,4 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -14,6 +14,5 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/head_test_cart/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/head_test_cart/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/head_test_cart/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -15,5 +15,4 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="rosrecord"/>
</package>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -19,7 +19,6 @@
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="dcam"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_arm/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_arm/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_arm/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -17,7 +17,6 @@
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="dcam"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_full/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_full/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_full/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -17,7 +17,6 @@
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="dcam"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_pan_tilt/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_pan_tilt/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_pan_tilt/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -16,7 +16,6 @@
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="dcam"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -15,7 +15,6 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="point_cloud_assembler" />
</package>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/manifest.xml 2009-03-13 01:50:10 UTC (rev 12485)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_prototype1/manifest.xml 2009-03-13 01:58:34 UTC (rev 12486)
@@ -17,7 +17,6 @@
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
<depend package="joy"/>
- <depend package="rosrecord"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
<depend package="dcam"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-03-14 21:22:40
|
Revision: 12520
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12520&view=rev
Author: isucan
Date: 2009-03-14 21:22:28 +0000 (Sat, 14 Mar 2009)
Log Message:
-----------
using new initialization option from ODE (recursive initialization is possible)
Modified Paths:
--------------
pkg/trunk/3rdparty/opende/Makefile
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
Modified: pkg/trunk/3rdparty/opende/Makefile
===================================================================
--- pkg/trunk/3rdparty/opende/Makefile 2009-03-14 02:31:59 UTC (rev 12519)
+++ pkg/trunk/3rdparty/opende/Makefile 2009-03-14 21:22:28 UTC (rev 12520)
@@ -2,7 +2,7 @@
WITH_DRAWSTUFF = yes
CFG_OPTIONS = --with-arch=nocona --enable-release --disable-asserts --with-pic #--enable-double-precision
-SVN_REVISION = -r 1657
+SVN_REVISION = -r 1658
SVN_PATCH = opende_patch.diff
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-03-14 02:31:59 UTC (rev 12519)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-03-14 21:22:28 UTC (rev 12520)
@@ -51,16 +51,9 @@
{
public:
- EnvironmentModelODE(bool performODEinit = true) : EnvironmentModel()
+ EnvironmentModelODE(void) : EnvironmentModel()
{
- static bool initODE = true;
- if (initODE)
- {
- if (performODEinit)
- dInitODE();
- initODE = false;
- }
-
+ dInitODE2(0);
m_space = dHashSpaceCreate(0);
m_spaceBasicGeoms = dHashSpaceCreate(0);
}
@@ -68,6 +61,7 @@
virtual ~EnvironmentModelODE(void)
{
freeMemory();
+ dCloseODE();
}
/** The space ID for the objects that can be changed in the
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-03-16 22:57:41
|
Revision: 12551
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12551&view=rev
Author: sfkwc
Date: 2009-03-16 22:57:40 +0000 (Mon, 16 Mar 2009)
Log Message:
-----------
moved cv_wrappers, katana, spectacles, sdl, irobot_create, svl-1.0, borg, cloud_viewer, and stair1-demos to sail-ros-pkg
Removed Paths:
-------------
pkg/trunk/3rdparty/sdl/
pkg/trunk/3rdparty/svl-1.0/
pkg/trunk/demos/stair1-demos/
pkg/trunk/deprecated/cv_wrappers/
pkg/trunk/drivers/robot/irobot_create/
pkg/trunk/drivers/robot/katana/
pkg/trunk/vision/borg/
pkg/trunk/vision/spectacles/
pkg/trunk/visualization/cloud_viewer/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-03-17 00:56:38
|
Revision: 12564
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12564&view=rev
Author: stuglaser
Date: 2009-03-17 00:56:35 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
Dump of changes made for plugging in.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/plug_in/manifest.xml
Added Paths:
-----------
pkg/trunk/demos/plug_in/CMakeLists.txt
pkg/trunk/demos/plug_in/Makefile
pkg/trunk/demos/plug_in/outlet_pose_filter.py
pkg/trunk/demos/plug_in/plug_in2.py
pkg/trunk/demos/plug_in/src/
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/prcore/robot_msgs/msg/CartesianState.msg
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-03-17 00:56:35 UTC (rev 12564)
@@ -56,6 +56,7 @@
virtual void update(void);
virtual bool starting();
+ KDL::Twist pose_desi_;
KDL::Twist twist_desi_;
KDL::Wrench wrench_desi_;
KDL::Frame pose_meas_;
@@ -75,6 +76,8 @@
KDL::Chain kdl_chain_;
mechanism::RobotState *robot_;
double last_time_;
+
+ int initial_mode_;
};
class CartesianHybridControllerNode : public Controller
@@ -100,8 +103,11 @@
ros::Node *node_;
robot_msgs::TaskFrameFormalism command_msg_;
+ std::string task_frame_name_;
+
unsigned int loop_count_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_msgs::CartesianState> > pub_state_;
+ boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-03-17 00:56:35 UTC (rev 12564)
@@ -40,6 +40,7 @@
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
#include "robot_msgs/VisualizationMarker.h"
+#include "angles/angles.h"
namespace controller {
@@ -122,6 +123,9 @@
for (size_t i = 3; i < 6; ++i)
twist_pids_[i] = temp_pid;
+ if (!node->getParam(name + "/initial_mode", initial_mode_))
+ initial_mode_ = robot_msgs::TaskFrameFormalism::FORCE;
+
// Default commands
task_frame_offset_ = KDL::Frame::Identity();
@@ -162,15 +166,18 @@
{
twist_desi_[i] = 0;
wrench_desi_[i] = 0;
+ pose_desi_[i] = 0;
double setpoint = setpoint_[i];
switch(mode_[i])
{
case robot_msgs::TaskFrameFormalism::POSITION:
+ pose_desi_[i] = setpoint;
if (i < 3) // Translational position
setpoint = pose_pids_[i].updatePid(tool.p.p[i] - setpoint, dt);
else // Rotational position
- setpoint = pose_pids_[i].updatePid(rpy[i - 3] - setpoint, dt);
+ //setpoint = pose_pids_[i].updatePid(angles::shortest_angular_distance(rpy[i - 3], setpoint), dt);
+ setpoint = pose_pids_[i].updatePid(angles::shortest_angular_distance(setpoint, rpy[i - 3]), dt);
case robot_msgs::TaskFrameFormalism::VELOCITY:
twist_desi_[i] = setpoint;
setpoint = twist_pids_[i].updatePid(tool.GetTwist()[i] - setpoint, dt);
@@ -208,11 +215,45 @@
bool CartesianHybridController::starting()
{
- for (size_t i = 0; i < 6; ++i)
+ task_frame_offset_ = KDL::Frame::Identity();
+ tool_frame_offset_ = KDL::Frame::Identity();
+
+
+ switch(initial_mode_)
{
- mode_[i] = robot_msgs::TaskFrameFormalism::FORCE;
- setpoint_[i] = 0.0;
+ case robot_msgs::TaskFrameFormalism::POSITION: {
+ // Finds the starting pose/twist
+ KDL::JntArrayVel jnt_vel(kdl_chain_.getNrOfJoints());
+ chain_.getVelocities(robot_->joint_states_, jnt_vel);
+ KDL::FrameVel frame;
+ KDL::ChainFkSolverVel_recursive fkvel_solver(kdl_chain_);
+ fkvel_solver.JntToCart(jnt_vel, frame);
+
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ }
+ for (size_t i = 0; i < 3; ++i) {
+ setpoint_[i] = frame.p.p[i];
+ }
+ frame.M.R.GetRPY(setpoint_[3], setpoint_[4], setpoint_[5]);
+ break;
}
+ case robot_msgs::TaskFrameFormalism::VELOCITY:
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ setpoint_[i] = 0.0;
+ }
+ break;
+ case robot_msgs::TaskFrameFormalism::FORCE:
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ setpoint_[i] = 0.0;
+ }
+ break;
+ default:
+ return false;
+ }
+
return true;
}
@@ -241,10 +282,14 @@
ros::Node *node = ros::Node::instance();
- node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 1);
+ task_frame_name_ = c_.chain_.getLinkName(0);
+
+ node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 5);
node->advertiseService(name_ + "/set_tool_frame", &CartesianHybridControllerNode::setToolFrame, this);
pub_state_.reset(new realtime_tools::RealtimePublisher<robot_msgs::CartesianState>(name_ + "/state", 1));
+ pub_tf_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>("/tf_message", 5));
+ pub_tf_->msg_.transforms.resize(1);
return true;
@@ -252,6 +297,8 @@
void CartesianHybridControllerNode::update()
{
+
+ KDL::Twist last_pose_desi = c_.pose_desi_;
KDL::Twist last_twist_desi = c_.twist_desi_;
KDL::Wrench last_wrench_desi = c_.wrench_desi_;
@@ -262,13 +309,26 @@
if (pub_state_->trylock())
{
pub_state_->msg_.header.frame_id = "TODO___THE_TASK_FRAME";
- TransformKDLToMsg(c_.pose_meas_, pub_state_->msg_.last_pose_meas);
+ TransformKDLToMsg(c_.pose_meas_, pub_state_->msg_.last_pose_meas.pose);
+ pub_state_->msg_.last_pose_meas.header.frame_id = task_frame_name_;
+ pub_state_->msg_.last_pose_meas.header.stamp = ros::Time(c_.last_time_);
+ TwistKDLToMsg(last_pose_desi, pub_state_->msg_.last_pose_desi);
TwistKDLToMsg(c_.twist_meas_, pub_state_->msg_.last_twist_meas);
TwistKDLToMsg(last_twist_desi, pub_state_->msg_.last_twist_desi);
WrenchKDLToMsg(last_wrench_desi, pub_state_->msg_.last_wrench_desi);
pub_state_->unlockAndPublish();
}
+ if (pub_tf_->trylock())
+ {
+ //pub_tf_->msg_.transforms[0].header.stamp.fromSec();
+ pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
+ pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
+ tf::Transform t;
+ mechanism::TransformKDLToTF(c_.tool_frame_offset_, t);
+ tf::TransformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
+ pub_tf_->unlockAndPublish();
+ }
}
}
@@ -318,8 +378,16 @@
void CartesianHybridControllerNode::command()
{
+ task_frame_name_ = command_msg_.header.frame_id;
tf::Stamped<tf::Transform> task_frame;
+
try {
+ ROS_INFO("Waiting on transform (%.3lf vs %.3lf)", command_msg_.header.stamp.toSec(), ros::Time::now().toSec());
+ while (!TF.canTransform(c_.chain_.getLinkName(0), command_msg_.header.frame_id, command_msg_.header.stamp))
+ {
+ usleep(10000);
+ }
+ ROS_INFO("Got transform.");
TF.lookupTransform(c_.chain_.getLinkName(0), command_msg_.header.frame_id, command_msg_.header.stamp,
task_frame);
}
Added: pkg/trunk/demos/plug_in/CMakeLists.txt
===================================================================
--- pkg/trunk/demos/plug_in/CMakeLists.txt (rev 0)
+++ pkg/trunk/demos/plug_in/CMakeLists.txt 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(plug_in)
+
+#rospack_add_boost_directories()
+rospack_add_executable(servo src/servo.cpp)
Added: pkg/trunk/demos/plug_in/Makefile
===================================================================
--- pkg/trunk/demos/plug_in/Makefile (rev 0)
+++ pkg/trunk/demos/plug_in/Makefile 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -27,9 +27,7 @@
# POSSIBILITY OF SUCH DAMAGE.
import roslib
-roslib.load_manifest('pr2_mechanism_controllers')
-roslib.load_manifest('std_msgs')
-roslib.load_manifest('rospy')
+roslib.load_manifest('plug_in')
import rospy
from std_msgs.msg import *
@@ -37,24 +35,17 @@
from tf.msg import tfMessage
from math import *
from time import sleep
+import tf.transformations
+
def xyz(x, y, z):
p = Point()
p.x, p.y, p.z = x, y, z
return p
-def rpy(y, p, r):
- q = Quaternion()
- cosY = cos(y / 2.)
- sinY = sin(y / 2.)
- cosP = cos(p / 2.)
- sinP = sin(p / 2.)
- cosR = cos(r / 2.)
- sinR = sin(r / 2.)
- q.x = cosR*cosP*cosY + sinR*sinP*sinY
- q.y = sinR*cosP*cosY + cosR*sinP*sinY
- q.z = cosR*sinP*cosY + sinR*cosP*sinY
- q.w = cosR*cosP*sinY + sinR*sinP*cosY
+def rpy(r, p, y):
+ a = tf.transformations.quaternion_from_euler(r, p, y, "rzyx")
+ q = Quaternion(a[0], a[1], a[2], a[3])
return q
@@ -68,6 +59,7 @@
mechanism_state = Tracker('/mechanism_state', MechanismState)
def last_time():
+ return rospy.rostime.get_rostime()
global mechanism_state
if mechanism_state.msg:
return mechanism_state.msg.header.stamp
@@ -75,23 +67,23 @@
-pub_outlet = rospy.Publisher('/outlet_pose', PoseStamped)
-pub_plug = rospy.Publisher('/plug_pose', PoseStamped)
+pub_outlet = rospy.Publisher('/outlet_detector/pose', PoseStamped)
+pub_plug = rospy.Publisher('/plug_detector/pose', PoseStamped)
rospy.init_node('fake_vision', anonymous=True)
-while last_time() == 0:
- pass
+sleep(0.2)
+
def send():
op = PoseStamped()
op.header.stamp = last_time()
op.header.frame_id = 'torso_lift_link'
- op.pose.position = xyz(0.7, -0.4, -0.2)
+ op.pose.position = xyz(0.7, -0.4, -0.4)
op.pose.orientation = rpy(0, 0, 0)
pp = PoseStamped()
pp.header.stamp = last_time()
pp.header.frame_id = 'r_gripper_tool_frame'
pp.pose.position = xyz(0.0, 0.0, 0.0)
- pp.pose.orientation = rpy(0,pi/6,0)
+ pp.pose.orientation = rpy(0,-pi/6,0)
print "Publishing..."
for i in range(10):
Modified: pkg/trunk/demos/plug_in/manifest.xml
===================================================================
--- pkg/trunk/demos/plug_in/manifest.xml 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/demos/plug_in/manifest.xml 2009-03-17 00:56:35 UTC (rev 12564)
@@ -9,4 +9,5 @@
<depend package="pr2_etherCAT" />
<depend package="joy" />
<depend package="outlet_detection" />
+ <depend package="tf" />
</package>
Added: pkg/trunk/demos/plug_in/outlet_pose_filter.py
===================================================================
--- pkg/trunk/demos/plug_in/outlet_pose_filter.py (rev 0)
+++ pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,87 @@
+#! /usr/bin/env python
+# 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.
+
+import sys, time
+import roslib; roslib.load_manifest('plug_in')
+import rospy
+from robot_msgs.msg import PoseStamped, TransformStamped
+import tf.msg
+from std_srvs.srv import Empty
+
+class Tracker:
+ def __init__(self, topic, Msg):
+ self.sub = rospy.Subscriber(topic, Msg, self.callback)
+ self.msg = None
+
+ def callback(self, msg):
+ self.msg = msg
+
+def pose_to_transform_stamped(p):
+ t = TransformStamped()
+ t.header.stamp = p.header.stamp
+ #t.header.frame_id = p.header.frame_id
+ t.parent_id = p.header.frame_id
+ t.transform.translation.x = p.pose.position.x
+ t.transform.translation.y = p.pose.position.y
+ t.transform.translation.z = p.pose.position.z
+ t.transform.rotation = p.pose.orientation
+ return t
+
+def main():
+
+ # TODO: outlet pose remains stable in the base frame. Transform
+ # to there, and then continuously publish
+
+ track_outlet_pose = Tracker('/outlet_detector/pose', PoseStamped)
+
+ def reset():
+ track_outlet_pose.msg = None
+ rospy.Service('/outlet_pose_filter/reset', Empty, reset)
+
+ tf_pub = rospy.Publisher('/tf_message', tf.msg.tfMessage)
+ tf_msg = tf.msg.tfMessage()
+ tf_msg.transforms = [TransformStamped()]
+
+ rospy.init_node('outlet_filter')
+
+ seq = 0
+ outlet_msg = None
+ while not rospy.is_shutdown():
+ if outlet_msg: # and outlet_msg.pose.orientation.z > 0: # HACK
+ tf_msg.transforms = [pose_to_transform_stamped(outlet_msg)]
+ tf_msg.transforms[0].header.seq = seq
+ tf_msg.transforms[0].header.frame_id = 'outlet_pose'
+ tf_msg.transforms[0].header.stamp = rospy.rostime.get_rostime()
+ tf_pub.publish(tf_msg)
+ seq += 1
+ else:
+ outlet_msg = track_outlet_pose.msg
+
+ time.sleep(0.01)
+
+if __name__ == '__main__': main()
Property changes on: pkg/trunk/demos/plug_in/outlet_pose_filter.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/demos/plug_in/plug_in2.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in2.py (rev 0)
+++ pkg/trunk/demos/plug_in/plug_in2.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,274 @@
+#! /usr/bin/env python
+# 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.
+
+# Author: Stuart Glaser
+
+import sys, time
+import math
+
+import roslib
+roslib.load_manifest('plug_in')
+import rospy
+from robot_msgs.msg import *
+from robot_srvs.srv import *
+import tf.transformations
+
+CONTROLLER = 'arm_constraint'
+
+class Tracker:
+ def __init__(self, topic, Msg):
+ self.sub = rospy.Subscriber(topic, Msg, self.callback)
+ self.msg = None
+
+ def callback(self, msg):
+ self.msg = msg
+
+
+
+mechanism_state = Tracker('/mechanism_state', MechanismState)
+def last_time():
+ global mechanism_state
+ if mechanism_state.msg:
+ return mechanism_state.msg.header.stamp
+ return 0
+
+def xyz(x, y, z):
+ p = Point()
+ p.x, p.y, p.z = x, y, z
+ return p
+
+def rpy(r, p, y):
+ a = tf.transformations.quaternion_from_euler(r, p, y, "rzyx")
+ q = Quaternion(a[0], a[1], a[2], a[3])
+ return q
+
+
+def main():
+ rospy.init_node('plug_in')
+
+ track_outlet_pose = Tracker('/outlet_detector/pose', PoseStamped)
+ track_plug_pose = Tracker('/plug_detector/pose', PoseStamped)
+
+ print "Waiting for mechanism control"
+ rospy.wait_for_service('kill_and_spawn_controllers')
+ kill_and_spawn = rospy.ServiceProxy('kill_and_spawn_controllers', KillAndSpawnControllers)
+ if rospy.is_shutdown(): sys.exit(0)
+
+ # Bring down all possible controllers, just in case
+ kill_and_spawn('<controllers></controllers>', ['arm_pose', CONTROLLER, 'arm_hybrid'])
+
+ ###### Finds the outlet
+
+ # Waits for an estimate of the outlet pose
+ print "Waiting for outlet pose..."
+ outlet_pose = None
+ msg = None
+ while not msg:
+ msg = track_outlet_pose.msg
+ if rospy.is_shutdown(): return
+ time.sleep(0.1)
+ outlet_pose = msg
+ print "Found outlet"
+
+ ###### Stages the plug
+
+ # Spawns the pose controller
+ print "Spawning the pose controller"
+ rospy.set_param("/arm_pose/p", 15.0)
+ rospy.set_param("/arm_pose/i", 0.2)
+ rospy.set_param("/arm_pose/d", 0.0)
+ rospy.set_param("/arm_pose/i_clamp", 0.5)
+ rospy.set_param("/arm_pose/root_name", "torso_lift_link")
+ rospy.set_param("/arm_pose/tip_name", "r_gripper_tool_frame")
+ rospy.set_param("/arm_pose/twist/fb_trans/p", 15.0)
+ rospy.set_param("/arm_pose/twist/fb_trans/i", 0.5)
+ rospy.set_param("/arm_pose/twist/fb_trans/d", 0.0)
+ rospy.set_param("/arm_pose/twist/fb_trans/i_clamp", 1.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/p", 1.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/i", 0.1)
+ rospy.set_param("/arm_pose/twist/fb_rot/d", 0.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/i_clamp", 0.2)
+ pose_config = '<controller name="arm_pose" type="CartesianPoseControllerNode" />'
+ resp = kill_and_spawn(pose_config, [])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the pose controller"
+
+ # Commands the hand to near the outlet
+ print "Staging the plug"
+ staging_pose = PoseStamped()
+ staging_pose.header.frame_id = 'outlet_pose'
+ staging_pose.pose.position = xyz(-0.1, 0.0, 0.0)
+ staging_pose.pose.orientation = rpy(0,0,0)
+ pub_pose = rospy.Publisher('/arm_pose/command', PoseStamped)
+ for i in range(50):
+ staging_pose.header.stamp = last_time()
+ pub_pose.publish(staging_pose)
+ time.sleep(0.1)
+ time.sleep(1)
+
+ ###### Finds an initial estimate of the plug pose
+
+ # TODO: should wait for the hand to settle
+ print "Waiting for the first plug pose estimate"
+ track_plug_pose.msg = None
+ while not track_plug_pose.msg:
+ if rospy.is_shutdown(): return
+ time.sleep(0.1)
+ plug_pose = track_plug_pose.msg
+ print "Found plug"
+
+ if False: # Constraint controller
+
+ ###### Switches to the constraint controller
+
+ print "Spawning the constraint controller"
+ rospy.set_param("/arm_constraint/line_pid/p", 750.0)
+ rospy.set_param("/arm_constraint/line_pid/i", 1.0)
+ rospy.set_param("/arm_constraint/line_pid/d", 5.0)
+ rospy.set_param("/arm_constraint/line_pid/i_clamp", 10.0)
+ rospy.set_param("/arm_constraint/pose_pid/p", 35.0)
+ rospy.set_param("/arm_constraint/pose_pid/i", 8.0)
+ rospy.set_param("/arm_constraint/pose_pid/d", 1.0)
+ rospy.set_param("/arm_constraint/pose_pid/i_clamp", 10.0)
+ rospy.set_param("/arm_constraint/f_r_max", 150.0)
+ constraint_config = open('controllers.xml').read()
+ resp = kill_and_spawn(constraint_config, ['arm_pose'])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the constraint controller"
+
+ print "Setting tool frame"
+ rospy.wait_for_service("/%s/set_tool_frame" % CONTROLLER)
+ if rospy.is_shutdown(): return
+ set_tool_frame = rospy.ServiceProxy("/%s/set_tool_frame" % CONTROLLER, SetPoseStamped)
+ set_tool_frame(plug_pose)
+ print "Tool frame set"
+ time.sleep(1)
+
+
+ ###### Visual differencing loop over the plug pose estimate
+
+ pub_command = rospy.Publisher("/%s/outlet_pose" % CONTROLLER, PoseStamped)
+ cnt = 0
+ while not rospy.is_shutdown():
+ cnt += 1
+ outlet_pose.header.stamp = last_time()
+ pub_command.publish(outlet_pose)
+ if cnt % 3 == 0:
+ set_tool_frame(track_plug_pose.msg)
+ time.sleep(1.0)
+
+ ###### Attempt to plug in
+
+ else:
+
+ ###### Switches to the hybrid controller
+
+ print "Spawning the hybrid controller"
+ rospy.set_param("/arm_hybrid/type", "CartesianHybridControllerNode")
+ rospy.set_param("/arm_hybrid/initial_mode", 3)
+ rospy.set_param("/arm_hybrid/root_link", "torso_lift_link")
+ rospy.set_param("/arm_hybrid/tip_link", "r_gripper_tool_frame")
+ rospy.set_param("/arm_hybrid/fb_pose/p", 30.0)#20.0)
+ rospy.set_param("/arm_hybrid/fb_pose/i", 4.0)
+ rospy.set_param("/arm_hybrid/fb_pose/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_pose/i_clamp", 2.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/p", 15.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/i", 1.5)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/i_clamp", 6.0)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/p", 1.2)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/i", 0.2)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/i_clamp", 0.4)
+ hybrid_config = '<controller name="arm_hybrid" type="CartesianHybridControllerNode" />'
+ resp = kill_and_spawn(hybrid_config, ['arm_pose'])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the hybrid controller"
+
+ ###### Sets the tool frame
+
+ print "Setting tool frame"
+ rospy.wait_for_service("/arm_hybrid/set_tool_frame")
+ if rospy.is_shutdown(): return
+ set_tool_frame = rospy.ServiceProxy("/arm_hybrid/set_tool_frame", SetPoseStamped)
+ set_tool_frame(plug_pose)
+ print "Tool frame set"
+ time.sleep(1)
+
+ ###### Visual differencing
+
+ track_state = Tracker("/arm_hybrid/state", CartesianState)
+
+ pub_hybrid = rospy.Publisher('/arm_hybrid/command', TaskFrameFormalism)
+ tff = TaskFrameFormalism()
+ tff.header.frame_id = 'outlet_pose'
+ tff.mode.vel.x = 3
+ tff.mode.vel.y = 3
+ tff.mode.vel.z = 3
+ tff.mode.rot.x = 3
+ tff.mode.rot.y = 3
+ tff.mode.rot.z = 3
+
+ while not rospy.is_shutdown():
+ time.sleep(0.1)
+ return
+
+ while not rospy.is_shutdown():
+
+ # Waits for the controller to settle
+ vel = 1
+ track_state.msg = None
+ while vel > 0.0001:
+ while not track_state.msg:
+ time.sleep(0.01)
+ if rospy.is_shutdown(): return
+ twist_msg = track_state.msg.last_twist_meas
+ v = math.sqrt(twist_msg.vel.x**2 + twist_msg.vel.y**2 + twist_msg.vel.z**2) + \
+ 0.1 * math.sqrt(twist_msg.rot.x**2 + twist_msg.rot.y**2 + twist_msg.rot.z**2)
+ vel = max(v, 0.1*v + 0.9*vel)
+ print "Vel:", vel
+ print "Settled"
+
+ tool_pose = track_state.msg.last_pose_meas
+
+ # Gets the next plug pose estimate
+ track_plug_pose.msg = None
+ while not track_plug_pose.msg:
+ time.sleep(0.01)
+ print "Got a new plug pose"
+
+ #TODO: plug_in_outlet = TF
+
+ # Determines the offset
+ #set_tool_frame(track_plug_pose.msg)
+
+ # Commands the controller to move
+ pub_hybrid.publish(tff)
+
+
+if __name__ == '__main__': main()
Property changes on: pkg/trunk/demos/plug_in/plug_in2.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp (rev 0)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,304 @@
+/*
+ * 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.
+ */
+
+#include "boost/scoped_ptr.hpp"
+#include "ros/node.h"
+#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
+
+#include "robot_msgs/PoseStamped.h"
+#include "robot_msgs/CartesianState.h"
+#include "robot_msgs/TaskFrameFormalism.h"
+
+enum {MEASURING, MOVING, PUSHING, FORCING};
+int g_state = MEASURING;
+
+ros::Time g_started_pushing, g_started_forcing;
+
+bool g_settled = false;
+robot_msgs::CartesianState g_hybrid_state;
+void hybrid_state_cb()
+{
+ static double speed = 1.0;
+
+ tf::Vector3 vel, rot;
+ tf::PointMsgToTF(g_hybrid_state.last_twist_meas.vel, vel);
+ tf::PointMsgToTF(g_hybrid_state.last_twist_meas.rot, rot);
+
+ double s = vel.length() + 0.05 * rot.length();
+ speed = s > speed ? s : (0.05 * s + 0.95 * speed);
+
+ //printf("Speed: %lf\n", speed);
+ if (!g_settled && speed < 0.4)
+ {
+ printf("Settled\n");
+ g_settled = true;
+ }
+ if (g_settled && speed > 0.5)
+ {
+ printf("Moving again\n");
+ g_settled = false;
+ }
+}
+
+boost::scoped_ptr<tf::TransformListener> TF;
+
+//robot_msgs::PoseStamped g_plug;
+void plug_cb(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg)
+{
+ if (!g_settled) {
+ printf("Not yet settled\n");
+ return;
+ }
+
+ // Both are transforms from the outlet to the estimated plug pose
+ robot_msgs::PoseStamped viz_offset_msg;
+ tf::Stamped<tf::Transform> mech_offset;
+ try {
+ TF->transformPose("outlet_pose", *(msg.get()), viz_offset_msg);
+ TF->lookupTransform("outlet_pose", "arm_hybrid/tool_frame", msg->header.stamp, mech_offset);
+ }
+ catch(tf::TransformException &ex)
+ {
+ printf("FAIL: %s\n", ex.what());
+ return;
+ }
+
+ tf::Pose viz_offset;
+ tf::PoseMsgToTF(viz_offset_msg.pose, viz_offset);
+
+ // The standoff is at least half the current distance
+ double standoff = std::max(0.03, viz_offset.getOrigin().length() / 2.0);
+
+ // Computes the offset for movement
+...
[truncated message content] |
|
From: <rob...@us...> - 2009-03-17 22:16:08
|
Revision: 12599
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12599&view=rev
Author: rob_wheeler
Date: 2009-03-17 22:16:00 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
You must be root to run.
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/src/motorconf.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/motorconf.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/motorconf.cpp 2009-03-17 21:47:33 UTC (rev 12598)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/motorconf.cpp 2009-03-17 22:16:00 UTC (rev 12599)
@@ -64,7 +64,7 @@
// Initialize network interface
if ((ni = init_ec(interface)) == NULL)
{
- fprintf(stderr, "Unable to initialize interface: %s", interface);
+ fprintf(stderr, "Unable to initialize interface: %s\n", interface);
exit(-1);
}
@@ -73,14 +73,14 @@
EtherCAT_AL *al;
if ((al = EtherCAT_AL::instance()) == NULL)
{
- fprintf(stderr, "Unable to initialize Application Layer (AL): %p", al);
+ fprintf(stderr, "Unable to initialize Application Layer (AL): %p\n", al);
exit(-1);
}
uint32_t num_slaves = al->get_num_slaves();
if (num_slaves == 0)
{
- fprintf(stderr, "Unable to locate any slaves");
+ fprintf(stderr, "Unable to locate any slaves\n");
exit(-1);
}
@@ -238,10 +238,16 @@
int main(int argc, char *argv[])
{
+ // Must run as root
+ if (geteuid() != 0)
+ {
+ fprintf(stderr, "You must run as root!\n");
+ exit(-1);
+ }
+
// Keep the kernel from swapping us out
mlockall(MCL_CURRENT | MCL_FUTURE);
- //
// Parse options
g_options.program_name_ = argv[0];
g_options.device_ = -1;
@@ -297,7 +303,6 @@
string filename = "actuators.conf";
if (g_options.actuators_ != "")
filename = g_options.actuators_;
- printf("Filename: %s\nWith:%s\n",filename.c_str(), g_options.actuators_.c_str());
TiXmlDocument xml(filename);
if (xml.LoadFile())
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-03-17 21:47:33 UTC (rev 12598)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2009-03-17 22:16:00 UTC (rev 12599)
@@ -440,6 +440,13 @@
static pthread_attr_t controlThreadAttr;
int main(int argc, char *argv[])
{
+ // Must run as root
+ if (geteuid() != 0)
+ {
+ fprintf(stderr, "You must run as root!\n");
+ exit(-1);
+ }
+
// Keep the kernel from swapping us out
mlockall(MCL_CURRENT | MCL_FUTURE);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-03-18 23:24:19
|
Revision: 12651
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12651&view=rev
Author: gerkey
Date: 2009-03-18 23:24:10 +0000 (Wed, 18 Mar 2009)
Log Message:
-----------
Changed printf/cout to ROS_*.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -63,7 +63,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in P3D \n");
+ ROS_DEBUG("Starting node in P3D");
}
}
@@ -161,7 +161,7 @@
void GazeboBattery::FiniChild()
{
- std::cout << "--------------- calling FiniChild in GazeboBattery --------------------" << std::endl;
+ ROS_DEBUG("Calling FiniChild in GazeboBattery");
rosnode_->unadvertise(this->stateTopicName_);
//rosnode_->unadvertise(this->diagnosticMessageTopicName_);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -69,7 +69,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in Gazebo Mechanism Control \n");
+ ROS_DEBUG("Starting node in Gazebo Mechanism Control");
}
if (getenv("CHECK_SPEEDUP"))
@@ -111,7 +111,7 @@
}
else
{
- fprintf(stderr, "WARNING (gazebo_mechanism_control): A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
+ ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
joints_.push_back(NULL);
}
@@ -184,9 +184,9 @@
catch (const char* c)
{
if (strcmp(c,"dividebyzero")==0)
- std::cout << "WARNING:pid controller reports divide by zero error" << std::endl;
+ ROS_WARN("pid controller reports divide by zero error");
else
- std::cout << "unknown const char* exception: " << c << std::endl;
+ ROS_WARN("unknown const char* exception: %s", c);
}
//--------------------------------------------------
@@ -226,7 +226,7 @@
void GazeboMechanismControl::FiniChild()
{
- std::cout << "--------------- calling FiniChild in GazeboMechanismControl --------------------" << std::endl;
+ ROS_DEBUG("Calling FiniChild in GazeboMechanismControl");
hw_.~HardwareInterface();
mc_.~MechanismControl();
@@ -246,19 +246,19 @@
// wait for robotdesc/pr2 on param server
while(tmp_param_string.empty())
{
- std::cout << "WARNING: gazebo mechanism control plugin is waiting for " << this->robotParam << " in param server. run merge/roslaunch send.xml or similar." << std::endl;
+ ROS_WARN("gazebo mechanism control plugin is waiting for %s in param server. run merge/roslaunch send.xml or similar.", this->robotParam.c_str());
this->rosnode_->getParam(this->robotParam,tmp_param_string);
usleep(100000);
}
- std::cout << "gazebo mechanism control got pr2.xml from param server, parsing it..." << std::endl;
+ ROS_INFO("gazebo mechanism control got pr2.xml from param server, parsing it...");
//std::cout << tmp_param_string << std::endl;
// initialize TiXmlDocument doc with a string
TiXmlDocument doc;
if (!doc.Parse(tmp_param_string.c_str()))
{
- fprintf(stderr, "Error: Could not load the gazebo mechanism_control plugin's configuration file: %s\n",
+ ROS_ERROR("Could not load the gazebo mechanism_control plugin's configuration file: %s\n",
tmp_param_string.c_str());
abort();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -66,7 +66,7 @@
// start a ros node if none exist
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in laser \n");
+ ROS_DEBUG("Starting node in laser");
}
}
@@ -81,7 +81,7 @@
void RosBlockLaser::LoadChild(XMLConfigNode *node)
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<robot_msgs::PointCloud>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_bumper.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -62,7 +62,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in bumper \n");
+ ROS_INFO("Starting node in bumper");
}
}
@@ -80,7 +80,9 @@
{
this->bumperTopicNameP->Load(node);
this->bumperTopicName = this->bumperTopicNameP->GetValue();
- std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
+ ROS_DEBUG("publishing contact/collisions to topic name: %s",
+ this->bumperTopicName.c_str());
+ //std::cout << " publishing contact/collisions to topic name: " << this->bumperTopicName << std::endl;
rosnode->advertise<std_msgs::String>(this->bumperTopicName+std::string("/info"),100);
rosnode->advertise<robot_msgs::Vector3Stamped>(this->bumperTopicName+std::string("/force"),100);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -70,7 +70,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in camera \n");
+ ROS_DEBUG("Starting node in camera");
}
// set buffer size
@@ -102,7 +102,7 @@
this->topicName = node->GetString("topicName","default_ros_camera",0); //read from xml file
this->frameName = node->GetString("frameName","default_ros_camera",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<deprecated_msgs::Image>(this->topicName,1);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -62,7 +62,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosF3D \n");
+ ROS_DEBUG("Starting node in RosF3D");
}
}
@@ -83,7 +83,7 @@
this->topicName = node->GetString("topicName", "", 1);
this->frameName = node->GetString("frameName", "", 1);
- std::cout << "==== topic name for RosF3D ======== " << this->topicName << std::endl;
+ ROS_DEBUG("==== topic name for RosF3D ======== %s", this->topicName.c_str());
rosnode->advertise<robot_msgs::Vector3Stamped>(this->topicName,10);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_joint_force.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -100,7 +100,7 @@
// // on each jointed object, respectively. These vectors have 4 elements: x,y,z
// // and a fourth one. So we transmit 4 dReals per vector.
int data_count = n_joints*4*4*sizeof(dReal);
- std::cout << "data_count:" << data_count << std::endl;
+ ROS_DEBUG("data_count: %d", data_count);
double data[1000];
@@ -115,7 +115,7 @@
memcpy(data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
}
- std::cout << " data: " << data << std::endl;
+ ROS_DEBUG_STREAM(" data: " << data << std::endl);
// this->myIface->Unlock();
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -66,7 +66,7 @@
// start a ros node if none exist
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in laser \n");
+ ROS_DEBUG("Starting node in laser");
}
}
@@ -81,7 +81,7 @@
void RosLaser::LoadChild(XMLConfigNode *node)
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
- std::cout << "================= " << this->topicName << std::endl;
+ ROS_DEBUG("================= %s", this->topicName.c_str());
rosnode->advertise<laser_scan::LaserScan>(this->topicName,10);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -59,7 +59,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosP3D \n");
+ ROS_DEBUG("Starting node in RosP3D");
}
}
@@ -87,7 +87,7 @@
this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
- std::cout << "==== topic name for RosP3D ======== " << this->topicName << std::endl;
+ ROS_DEBUG("==== topic name for RosP3D ======== %s", this->topicName.c_str());
if (this->topicName != "")
rosnode->advertise<robot_msgs::PoseWithRatesStamped>(this->topicName,10);
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_ptz.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -75,7 +75,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in camera \n");
+ ROS_DEBUG("Starting node in camera");
}
}
@@ -121,8 +121,8 @@
this->panFrameName = this->panJointNameP->GetValue();
this->tiltFrameName = this->tiltJointNameP->GetValue();
- std::cout << " publishing state topic for ptz " << this->stateTopicName << std::endl;
- std::cout << " subscribing command topic for ptz " << this->commandTopicName << std::endl;
+ ROS_DEBUG(" publishing state topic for ptz %s", this->stateTopicName.c_str());
+ ROS_DEBUG(" subscribing command topic for ptz %s", this->commandTopicName.c_str());
rosnode->advertise<axis_cam::PTZActuatorState>(this->stateTopicName,10);
rosnode->subscribe( commandTopicName, PTZControlMessage, &RosPTZ::PTZCommandReceived,this,10);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -65,7 +65,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in stereo camera \n");
+ ROS_DEBUG("Starting node in stereo camera");
}
}
@@ -110,7 +110,7 @@
this->leftFrameName = node->GetString("leftFrameName","default_ros_stereocamera_left_frame",0); //read from xml file
this->rightFrameName = node->GetString("rightFrameName","default_ros_stereocamera_right_frame",0); //read from xml file
- std::cout << "================= " << this->leftCloudTopicName << std::endl;
+ ROS_DEBUG("================= %s", this->leftCloudTopicName.c_str());
rosnode->advertise<robot_msgs::PointCloud>(this->leftCloudTopicName, 1);
rosnode->advertise<robot_msgs::PointCloud>(this->rightCloudTopicName, 1);
rosnode->advertise<deprecated_msgs::Image>(this->leftTopicName, 1);
@@ -228,7 +228,7 @@
this->leftCloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( stereo_data->head.time - this->leftCloudMsg.header.stamp.sec) );
this->leftCloudMsg.set_pts_size(sizeFloat);
this->leftCloudMsg.set_chan_size(sizeFloat);
- std::cout << " stereo size " << sizeFloat << std::endl;
+ ROS_DEBUG(" stereo size %d", sizeFloat);
// which way first?
for (unsigned int c=0; c< stereo_data->width; c++)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_time.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -56,7 +56,7 @@
// this only works for a single camera.
ros::init(argc,argv);
rosnode_ = new ros::Node("ros_gazebo",ros::Node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in RosTime \n");
+ ROS_DEBUG("Starting node in RosTime");
}
// for rostime
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -123,7 +123,7 @@
ROS_INFO("-------------------- starting node for pr2 param server factory \n");
std::string xml_content;
rosnode->getParam(argv[1],xml_content);
- ROS_INFO("robotdesc/pr2 content\n%s\n",xml_content.c_str());
+ ROS_DEBUG("%s content\n%s\n", argv[1], xml_content.c_str());
// Parse URDF from param server
bool enforce_limits = true;
@@ -177,7 +177,7 @@
}
//std::cout << " ------------------- xml ------------------- " << std::endl;
- ROS_INFO("converted to gazebo format\n%s\n",xml_string.c_str());
+ ROS_DEBUG("converted to gazebo format\n%s\n",xml_string.c_str());
//std::cout << " ------------------- xml ------------------- " << std::endl;
factoryIface->Lock(1);
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2009-03-18 23:20:58 UTC (rev 12650)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2009-03-18 23:24:10 UTC (rev 12651)
@@ -106,7 +106,7 @@
const char *name = elt->Attribute("name");
if (!name)
{
- fprintf(stderr, "Error: unnamed joint found\n");
+ ROS_ERROR("unnamed joint found\n");
return false;
}
name_ = name;
@@ -114,7 +114,7 @@
const char *type = elt->Attribute("type");
if (!type)
{
- fprintf(stderr, "Error: Joint \"%s\" has no type.\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" has no type.\n", name_.c_str());
return false;
}
type_ = g_type_map[type];
@@ -125,13 +125,13 @@
limits = elt->FirstChildElement("limit");
if (!limits)
{
- fprintf(stderr, "Error: Joint \"%s\" has no limits specified.\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" has no limits specified.\n", name_.c_str());
return false;
}
if (limits->QueryDoubleAttribute("effort", &effort_limit_) != TIXML_SUCCESS)
{
- fprintf(stderr, "Error: no effort limit specified for joint \"%s\"\n", name_.c_str());
+ ROS_ERROR("no effort limit specified for joint \"%s\"\n", name_.c_str());
return false;
}
@@ -141,7 +141,7 @@
{
if (limits->QueryDoubleAttribute("k_velocity", &k_velocity_limit_) != TIXML_SUCCESS)
{
- fprintf(stderr, "No k_velocity for joint %s\n", name_.c_str());
+ ROS_ERROR("No k_velocity for joint %s\n", name_.c_str());
return false;
}
}
@@ -151,7 +151,7 @@
if(calibration)
{
if(calibration->QueryDoubleAttribute("reference_position", &reference_position_) == TIXML_SUCCESS)
- std::cout<<"Found reference point at "<<reference_position_<<std::endl;
+ ROS_DEBUG_STREAM("Found reference point at " <<reference_position_<<std::endl);
}
@@ -164,18 +164,18 @@
}
else if (min_ret == TIXML_NO_ATTRIBUTE || max_ret == TIXML_NO_ATTRIBUTE)
{
- fprintf(stderr, "Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
+ ROS_ERROR("Error: no min and max limits specified for joint \"%s\"\n", name_.c_str());
return false;
}
if (type_ == JOINT_ROTARY || type_ == JOINT_PRISMATIC)
{
if (limits->QueryDoubleAttribute("k_position", &k_position_limit_) != TIXML_SUCCESS)
- fprintf(stderr, "No k_position for joint %s\n", name_.c_str());
+ ROS_DEBUG("No k_position for joint %s\n", name_.c_str());
if (limits->QueryDoubleAttribute("safety_length_min", &safety_length_min_) != TIXML_SUCCESS)
- fprintf(stderr, "No safety_length_min_ for joint %s\n", name_.c_str());
+ ROS_DEBUG("No safety_length_min_ for joint %s\n", name_.c_str());
if (limits->QueryDoubleAttribute("safety_length_max", &safety_length_max_) != TIXML_SUCCESS)
- fprintf(stderr, "No safety_lenght_max_ for joint %s\n", name_.c_str());
+ ROS_DEBUG("No safety_lenght_max_ for joint %s\n", name_.c_str());
has_safety_limits_ = true;
}
@@ -186,16 +186,16 @@
TiXmlElement *prop_el = elt->FirstChildElement("joint_properties");
if (!prop_el)
{
- fprintf(stderr, "Warning: Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
+ ROS_WARN("Joint \"%s\" did not specify any joint properties, default to 0.\n", name_.c_str());
joint_damping_coefficient_ = 0.0;
joint_friction_coefficient_ = 0.0;
}
else
{
if (prop_el->QueryDoubleAttribute("damping", &joint_damping_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"damping is not specified\n");
+ ROS_DEBUG("damping is not specified\n");
if (prop_el->QueryDoubleAttribute("friction", &joint_friction_coefficient_) != TIXML_SUCCESS)
- fprintf(stderr,"friction is not specified\n");
+ ROS_DEBUG("friction is not specified\n");
}
if (type_ == JOINT_ROTARY || type_ == JOINT_CONTINUOUS || type_ == JOINT_PRISMATIC)
@@ -204,14 +204,14 @@
TiXmlElement *axis_el = elt->FirstChildElement("axis");
if (!axis_el)
{
- fprintf(stderr, "Error: Joint \"%s\" did not specify an axis\n", name_.c_str());
+ ROS_ERROR("Joint \"%s\" did not specify an axis\n", name_.c_str());
return false;
}
std::vector<double> axis_pieces;
urdf::queryVectorAttribute(axis_el, "xyz", &axis_pieces);
if (axis_pieces.size() != 3)
{
- fprintf(stderr, "Error: The axis for joint \"%s\" must have 3 value\n", name_.c_str());
+ ROS_ERROR("The axis for joint \"%s\" must have 3 value\n", name_.c_str());
return false;
}
axis_[0] = axis_pieces[0];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-03-19 00:46:31
|
Revision: 12667
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12667&view=rev
Author: jfaustwg
Date: 2009-03-19 00:46:16 +0000 (Thu, 19 Mar 2009)
Log Message:
-----------
* Add a Header to the Polyline2D message.
* Fix point cloud and laser scan displays to clear themselves on fixed frame change
* Clear all the displays before deleting the render panel to fix a crash on exit
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * 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
@@ -101,7 +101,7 @@
* - @b "~costmap_2d/zLB" : an upper bound on the height of observations used for free space projection
* - @b "~costmap_2d/zUB" : a upper bound on the height of observations used for free space projection
* - @b "~costmap_2d/raytrace_window" : the size of the window in which free space will be cleared
- * - @b "~costmap_2d/raytrace_range" : the range after which points will be considered
+ * - @b "~costmap_2d/raytrace_range" : the range after which points will be considered
* - @b "~costmap_2d/obstacle_range" : obstacles outside this range will not be included in the cost map
* - @b "~trajectory_rollout/map_size" : the size of the window around the robot for evaluating local trajectories
* - @b "~trajectory_rollout/yaw_goal_tolerance" : control parameter for trajectory selection
@@ -173,7 +173,7 @@
ros::Node::instance()->param("/global_frame_id", global_frame_, std::string("/map"));
ros::Node::instance()->param("~costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
ros::Node::instance()->param("~costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
-
+
// Unsigned chars cannot be stored in parameter server
int tmpLethalObstacleThreshold;
ros::Node::instance()->param("~costmap_2d/lethal_obstacle_threshold", tmpLethalObstacleThreshold, int(lethalObstacleThreshold));
@@ -204,7 +204,7 @@
robotWidth_ = inscribedRadius * 2;
xy_goal_tolerance_ = robotWidth_ / 2;
- // Obtain parameters for sensors and allocate observation buffers accordingly. Rates are in Hz.
+ // Obtain parameters for sensors and allocate observation buffers accordingly. Rates are in Hz.
double base_laser_update_rate(2.0);
double tilt_laser_update_rate(2.0);
double low_obstacle_update_rate(0.2);
@@ -229,7 +229,7 @@
ros::Node::instance()->param("~costmap_2d/body_part_scale", bodypartScale, bodypartScale);
ros::Node::instance()->param("~costmap_2d/robot_name", robotName, robotName);
ros::Node::instance()->param("~costmap_2d/filter_robot_points", useFilter, useFilter);
-
+
if (useFilter) {
filter_ = new robot_filter::RobotFilter((ros::Node*)this, robotName, true, bodypartScale);
filter_->loadRobotDescription();
@@ -239,24 +239,24 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
- ros::Duration().fromSec(base_laser_keepalive),
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
- ros::Duration().fromSec(base_laser_keepalive),
+ baseCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
+ ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
- ros::Duration().fromSec(tilt_laser_keepalive),
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
+ ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
- ros::Duration().fromSec(low_obstacle_keepalive),
+ lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
+ ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
- ros::Duration().fromSec(stereo_keepalive),
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
+ ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -291,9 +291,9 @@
ros::Node::instance()->param("~costmap_2d/obstacle_range", obstacleRange, 10.0);
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
- inputData , resp.map.resolution,
+ inputData , resp.map.resolution,
lethalObstacleThreshold, maxZ_, zLB, zUB,
- inflationRadius, circumscribedRadius, inscribedRadius, weight,
+ inflationRadius, circumscribedRadius, inscribedRadius, weight,
obstacleRange, rayTraceRange, raytraceWindow);
// set up costmap service response
@@ -339,7 +339,7 @@
pt.y = 0;
footprint_.push_back(pt);
- controller_ = new trajectory_rollout::TrajectoryControllerROS(*ros::Node::instance(), tf_, global_frame_, *local_map_accessor_,
+ controller_ = new trajectory_rollout::TrajectoryControllerROS(*ros::Node::instance(), tf_, global_frame_, *local_map_accessor_,
footprint_, inscribedRadius, circumscribedRadius);
// Advertize messages to publish cost map updates
@@ -370,13 +370,13 @@
// The cost map is populated with either laser scans in the case that we are unable to use a
// world model source, or point clouds if we are. We shall pick one, and will be dominated by
// point clouds
- baseScanNotifier_ = new tf::MessageNotifier<laser_scan::LaserScan>(&tf_, ros::Node::instance(),
- boost::bind(&MoveBase::baseScanCallback, this, _1),
- "base_scan", global_frame_, 50);
- baseCloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
- boost::bind(&MoveBase::baseCloudCallback, this, _1),
- "base_scan_filtered", global_frame_, 50);
- tiltLaserNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
+ baseScanNotifier_ = new tf::MessageNotifier<laser_scan::LaserScan>(&tf_, ros::Node::instance(),
+ boost::bind(&MoveBase::baseScanCallback, this, _1),
+ "base_scan", global_frame_, 50);
+ baseCloudNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
+ boost::bind(&MoveBase::baseCloudCallback, this, _1),
+ "base_scan_filtered", global_frame_, 50);
+ tiltLaserNotifier_ = new tf::MessageNotifier<robot_msgs::PointCloud>(&tf_, ros::Node::instance(),
boost::bind(&MoveBase::tiltCloudCallback, this, _1),
"tilt_laser_cloud_filtered", global_frame_, 50);
ros::Node::instance()->subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, this, 1);
@@ -403,7 +403,7 @@
if(local_map_accessor_ != NULL)
delete local_map_accessor_;
-
+
if(global_map_accessor_ != NULL)
delete global_map_accessor_;
@@ -421,7 +421,7 @@
delete tiltLaserNotifier_;
}
- void MoveBase::updateGlobalPose(){
+ void MoveBase::updateGlobalPose(){
tf::Stamped<tf::Pose> robotPose;
robotPose.setIdentity();
robotPose.frame_id_ = "base_link";
@@ -494,7 +494,7 @@
// Get the current robot pose in the map frame
updateGlobalPose();
- // Assign state data
+ // Assign state data
stateMsg.pos.x = global_pose_.getOrigin().x();
stateMsg.pos.y = global_pose_.getOrigin().y();
double uselessPitch, uselessRoll, yaw;
@@ -572,7 +572,7 @@
try
{
tf::Stamped<btVector3> v_in(btVector3(odomMsg_.vel.x, odomMsg_.vel.y, 0), ros::Time(), odomMsg_.header.frame_id), v_out;
- tf_.transformVector("base_link", ros::Time(), v_in, odomMsg_.header.frame_id, v_out);
+ tf_.transformVector("base_link", ros::Time(), v_in, odomMsg_.header.frame_id, v_out);
base_odom_.vel.x = v_in.x();
base_odom_.vel.y = v_in.y();
base_odom_.vel.th = odomMsg_.vel.th;
@@ -647,6 +647,7 @@
void MoveBase::publishFootprint(double x, double y, double th){
std::vector<deprecated_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
robot_msgs::Polyline2D footprint_msg;
+ footprint_msg.header.frame_id = global_frame_;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
footprint_msg.color.g = 0;
@@ -661,6 +662,7 @@
void MoveBase::publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path) {
robot_msgs::Polyline2D guiPathMsg;
+ guiPathMsg.header.frame_id = global_frame_;
guiPathMsg.set_points_size(path.size());
unsigned int i = 0;
@@ -699,7 +701,7 @@
// and we have stopped the robot, then we are done
double uselessPitch, uselessRoll, yaw;
global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
- if(plan_.empty() &&
+ if(plan_.empty() &&
fabs(angles::shortest_angular_distance(yaw , stateMsg.goal.th)) < this->yaw_goal_tolerance_){ /// @todo: this is still wrong, should use bt or similar to check shortest angular distance of roll/pitch/yaw.
ROS_DEBUG("Goal achieved at: (%f, %f, %f) for (%f, %f, %f)\n",
@@ -736,7 +738,7 @@
bool MoveBase::checkWatchDog() const {
bool ok = baseScanBuffer_->isCurrent() && tiltScanBuffer_->isCurrent() && stereoCloudBuffer_->isCurrent() && lowObstacleBuffer_->isCurrent();
- if(!ok)
+ if(!ok)
ROS_INFO("Missed required cost map update. Should not allow commanding now. Check cost map data source.\n");
return ok;
@@ -747,14 +749,14 @@
* at least, this leads to poor performance since planning ks slow (seconds) and thus the robot stops alot. If we leave the
* velocity controller flexibility to work around the path in collision then that seems to work better. Note that this is still
* sensitive to the goal (exit point of the path in the window) being in collision which would require an alternate metric
- * to allow more flexibility to get near the goal - essentially treating the goal as a waypoint.
+ * to allow more flexibility to get near the goal - essentially treating the goal as a waypoint.
*/
bool MoveBase::dispatchCommands(){
// First criteria is that we have had a sufficiently recent sensor update to trust perception and that we have a valid plan. This latter
- // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead.
+ // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead.
// We want to be able to stop the robot in that case
bool planOk = checkWatchDog() && isValid();
- robot_msgs::PoseDot cmdVel; // Commanded velocities
+ robot_msgs::PoseDot cmdVel; // Commanded velocities
// if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
// orientation
@@ -903,6 +905,7 @@
// First publish raw obstacles in red
robot_msgs::Polyline2D pointCloudMsg;
+ pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
pointCloudMsg.color.a = 0.0;
@@ -915,8 +918,8 @@
pointCloudMsg.points[i].y = rawObstacles[i].second;
}
- if (!ros::Node::instance()->ok()) {
- return;
+ if (!ros::Node::instance()->ok()) {
+ return;
}
ros::Node::instance()->publish("raw_obstacles", pointCloudMsg);
@@ -933,8 +936,8 @@
pointCloudMsg.points[i].y = inflatedObstacles[i].second;
}
- if (!ros::Node::instance()->ok()) {
- return;
+ if (!ros::Node::instance()->ok()) {
+ return;
}
ros::Node::instance()->publish("inflated_obstacles", pointCloudMsg);
}
@@ -968,6 +971,7 @@
// First publish raw obstacles in red
robot_msgs::Polyline2D pointCloudMsg;
+ pointCloudMsg.header.frame_id = global_frame_;
unsigned int pointCount = rawObstacles.size();
pointCloudMsg.set_points_size(pointCount);
pointCloudMsg.color.a = 0.0;
@@ -1014,14 +1018,14 @@
MoveBase::footprint_t const & MoveBase::getFootprint() const{
return footprint_;
}
-
+
void MoveBase::updateCostMap() {
if (reset_cost_map_) {
costMap_->revertToStaticMap(global_pose_.getOrigin().x(), global_pose_.getOrigin().y());
}
ROS_DEBUG("Starting cost map update/n");
-
+
lock();
// Aggregate buffered observations across all sources. Must be thread safe
std::vector<costmap_2d::Observation> observations, raytrace_obs;
@@ -1049,7 +1053,7 @@
}
unlock();
-
+
ROS_DEBUG("Applying update with %d observations/n", stored_observations.size());
// Apply to cost map
ros::Time start = ros::Time::now();
@@ -1059,7 +1063,7 @@
costMap_->updateDynamicObstacles(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), stored_observations, &raytrace_obs.front());
double t_diff = (ros::Time::now() - start).toSec();
ROS_DEBUG("Updated map in %f seconds for %d observations/n", t_diff, stored_observations.size());
-
+
// Finally, we must extract the cost data that we have computed and:
// 1. Refresh the local_map_accessor for the controller
// 2. Refresh the global_map accessor for the planner
Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -38,12 +38,12 @@
using namespace trajectory_rollout;
namespace trajectory_rollout{
- GovernorNode::GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec) :
- ros::Node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
- tf_(*this, true, ros::Duration(10)),
+ GovernorNode::GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec) :
+ ros::Node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
+ tf_(*this, true, ros::Duration(10)),
ma_(map_, OUTER_RADIUS),
cm_(ma_),
- tc_(cm_, ma_, footprint_spec, ROBOT_SIDE_RADIUS, OUTER_RADIUS, MAX_ACC_X, MAX_ACC_Y, MAX_ACC_THETA,
+ tc_(cm_, ma_, footprint_spec, ROBOT_SIDE_RADIUS, OUTER_RADIUS, MAX_ACC_X, MAX_ACC_Y, MAX_ACC_THETA,
SIM_TIME, SIM_RES, VEL_SAMPLES, VEL_SAMPLES, PDIST_SCALE, GDIST_SCALE, OCCDIST_SCALE),
cycle_time_(0.1)
{
@@ -151,6 +151,7 @@
printf("Robot Vel - vx: %.2f, vy: %.2f, vth: %.2f\n", robot_vel.getOrigin().getX(), robot_vel.getOrigin().getY(), yaw);
if(path.cost_ >= 0){
+ path_msg.header.frame_id = "/map";
//let's print debug output to the screen
path_msg.set_points_size(path.getPointsSize());
path_msg.color.r = 0;
@@ -163,12 +164,12 @@
for(unsigned int i = 0; i < path.getPointsSize(); ++i){
double pt_x, pt_y, pt_th;
path.getPoint(i, pt_x, pt_y, pt_th);
- path_msg.points[i].x = pt_x;
+ path_msg.points[i].x = pt_x;
path_msg.points[i].y = pt_y;
//so we can draw the footprint on the map
if(i == 0){
- x = pt_x;
+ x = pt_x;
y = pt_y;
th = pt_th;
}
@@ -178,6 +179,7 @@
vector<deprecated_msgs::Point2DFloat32> footprint = tc_.drawFootprint(x, y, th);
//let's also draw the footprint of the robot for the last point on the selected trajectory
+ footprint_msg.header.frame_id = "/map";
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
footprint_msg.color.g = 0;
@@ -238,7 +240,7 @@
pt.x = -1 * ROBOT_FRONT_RADIUS;
pt.y = ROBOT_SIDE_RADIUS;
footprint_spec.push_back(pt);
-
+
GovernorNode gn(footprint_spec);
ros::Time start;
@@ -256,6 +258,6 @@
//printf("Cycle Time: %.2f\n", t_diff);
}
-
+
return(0);
}
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-03-19 00:46:16 UTC (rev 12667)
@@ -260,7 +260,7 @@
wn.doOneCycle();
wn.sleep(curr.tv_sec+curr.tv_usec/1e6);
}
-
+
return(0);
}
@@ -519,6 +519,7 @@
// Draw the points
+ this->pointcloudMsg.header.frame_id = "/map";
this->pointcloudMsg.set_points_size(this->laser_hitpts_len);
this->pointcloudMsg.color.a = 0.0;
this->pointcloudMsg.color.r = 0.0;
@@ -725,6 +726,7 @@
assert(this->plan->path_count);
+ this->polylineMsg.header.frame_id = "/map";
this->polylineMsg.set_points_size(this->plan->path_count);
this->polylineMsg.color.r = 0;
this->polylineMsg.color.g = 1.0;
Modified: pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg 2009-03-19 00:46:16 UTC (rev 12667)
@@ -1,2 +1,3 @@
+Header header
deprecated_msgs/Point2DFloat32[] points
std_msgs/ColorRGBA color
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/laser_scan_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -132,6 +132,8 @@
void LaserScanDisplay::fixedFrameChanged()
{
notifier_->setTargetFrame( fixed_frame_ );
+
+ PointCloudBase::fixedFrameChanged();
}
void LaserScanDisplay::createProperties()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/point_cloud_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -119,6 +119,8 @@
void PointCloudDisplay::fixedFrameChanged()
{
notifier_->setTargetFrame( fixed_frame_ );
+
+ PointCloudBase::fixedFrameChanged();
}
void PointCloudDisplay::createProperties()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -36,6 +36,7 @@
#include <ros/node.h>
#include <tf/transform_listener.h>
+#include <tf/message_notifier.h>
#include <boost/bind.hpp>
@@ -82,6 +83,8 @@
setAlpha( 1.0f );
setPointSize( 0.05f );
setZPosition( 0.0f );
+
+ notifier_ = new tf::MessageNotifier<robot_msgs::Polyline2D>(tf_, ros_node_, boost::bind(&PolyLine2DDisplay::incomingMessage, this, _1), "", "", 10);
}
PolyLine2DDisplay::~PolyLine2DDisplay()
@@ -92,6 +95,7 @@
scene_manager_->destroyManualObject( manual_object_ );
delete cloud_;
+ delete notifier_;
}
void PolyLine2DDisplay::clear()
@@ -218,7 +222,7 @@
if ( !topic_.empty() )
{
- ros_node_->subscribe( topic_, message_, &PolyLine2DDisplay::incomingMessage, this, 1 );
+ notifier_->setTopic(topic_);
}
ros_node_->subscribe( "map_metadata", metadata_message_, &PolyLine2DDisplay::incomingMetadataMessage, this, 1 );
@@ -226,10 +230,7 @@
void PolyLine2DDisplay::unsubscribe()
{
- if ( !topic_.empty() )
- {
- ros_node_->unsubscribe( topic_, &PolyLine2DDisplay::incomingMessage, this );
- }
+ notifier_->setTopic("");
ros_node_->unsubscribe( "map_metadata", &PolyLine2DDisplay::incomingMetadataMessage, this );
}
@@ -250,16 +251,18 @@
void PolyLine2DDisplay::fixedFrameChanged()
{
clear();
+
+ notifier_->setTargetFrame( fixed_frame_ );
}
void PolyLine2DDisplay::update( float dt )
{
if ( new_message_ )
{
+ new_message_ = false;
+
processMessage();
- new_message_ = false;
-
causeRender();
}
@@ -271,14 +274,30 @@
void PolyLine2DDisplay::processMessage()
{
- message_.lock();
+ boost::shared_ptr<robot_msgs::Polyline2D> msg;
+ {
+ boost::mutex::scoped_lock lock(message_mutex_);
+ msg = message_;
+ }
+
clear();
+ if (!msg)
+ {
+ return;
+ }
+
+ std::string frame_id = msg->header.frame_id;
+ if (frame_id.empty())
+ {
+ frame_id = "/map";
+ }
+
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0.0f, 0.0f, 0.0f ), btVector3( 0.0f, 0.0f, z_position_ ) ),
- ros::Time(), "map" );
+ ros::Time(), frame_id);
- if (tf_->canTransform(fixed_frame_, "map", ros::Time()))
+ if (tf_->canTransform(fixed_frame_, frame_id, ros::Time()))
{
try
{
@@ -312,10 +331,10 @@
}
else
{
- color = Ogre::ColourValue( message_.color.r, message_.color.g, message_.color.b, alpha_ );
+ color = Ogre::ColourValue( msg->color.r, msg->color.g, msg->color.b, alpha_ );
}
- uint32_t num_points = message_.get_points_size();
+ uint32_t num_points = msg->get_points_size();
if ( render_operation_ == poly_line_render_ops::Points )
{
typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
@@ -325,9 +344,9 @@
{
ogre_tools::PointCloud::Point& current_point = points[ i ];
- current_point.x_ = -message_.points[i].y;
+ current_point.x_ = -msg->points[i].y;
current_point.y_ = 0.0f;
- current_point.z_ = -message_.points[i].x;
+ current_point.z_ = -msg->points[i].x;
current_point.r_ = color.r;
current_point.g_ = color.g;
current_point.b_ = color.b;
@@ -346,27 +365,27 @@
manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( uint32_t i=0; i < num_points; ++i)
{
- manual_object_->position(-message_.points[i].y, 0.0f, -message_.points[i].x);
+ manual_object_->position(-msg->points[i].y, 0.0f, -msg->points[i].x);
manual_object_->colour( color );
}
if ( loop_ && num_points > 0 )
{
- manual_object_->position(-message_.points[num_points - 1].y, 0.0f, -message_.points[num_points - 1].x);
+ manual_object_->position(-msg->points[num_points - 1].y, 0.0f, -msg->points[num_points - 1].x);
manual_object_->colour( color );
- manual_object_->position(-message_.points[0].y, 0.0f, -message_.points[0].x);
+ manual_object_->position(-msg->points[0].y, 0.0f, -msg->points[0].x);
manual_object_->colour( color );
}
manual_object_->end();
}
-
- message_.unlock();
}
-void PolyLine2DDisplay::incomingMessage()
+void PolyLine2DDisplay::incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg)
{
+ boost::mutex::scoped_lock lock(message_mutex_);
new_message_ = true;
+ message_ = msg;
}
void PolyLine2DDisplay::incomingMetadataMessage()
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/poly_line_2d_display.h 2009-03-19 00:46:16 UTC (rev 12667)
@@ -37,6 +37,8 @@
#include <robot_msgs/Polyline2D.h>
#include <robot_msgs/MapMetaData.h>
+#include <boost/shared_ptr.hpp>
+
namespace ogre_tools
{
class PointCloud;
@@ -48,6 +50,11 @@
class ManualObject;
}
+namespace tf
+{
+template<class Message> class MessageNotifier;
+}
+
namespace rviz
{
@@ -119,7 +126,7 @@
void subscribe();
void unsubscribe();
void clear();
- void incomingMessage();
+ void incomingMessage(const boost::shared_ptr<robot_msgs::Polyline2D>& msg);
void incomingMetadataMessage();
void processMessage();
@@ -141,7 +148,9 @@
ogre_tools::PointCloud* cloud_;
bool new_message_;
- robot_msgs::Polyline2D message_;
+ boost::shared_ptr<robot_msgs::Polyline2D> message_;
+ boost::mutex message_mutex_;
+ tf::MessageNotifier<robot_msgs::Polyline2D>* notifier_;
bool new_metadata_;
robot_msgs::MapMetaData metadata_message_;
Modified: pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp 2009-03-19 00:43:05 UTC (rev 12666)
+++ pkg/trunk/visualization/rviz/src/rviz/visualization_frame.cpp 2009-03-19 00:46:16 UTC (rev 12667)
@@ -93,6 +93,8 @@
{
saveConfigs();
+ manager_->removeAllDisplays();
+
render_panel_->Destroy();
delete manager_;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-03-20 00:11:42
|
Revision: 12720
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12720&view=rev
Author: gerkey
Date: 2009-03-20 00:11:39 +0000 (Fri, 20 Mar 2009)
Log Message:
-----------
Lowered verbosity of some common log messages
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-03-20 00:06:30 UTC (rev 12719)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp 2009-03-20 00:11:39 UTC (rev 12720)
@@ -70,7 +70,7 @@
{
JointPDController * jpc = new JointPDController();
// std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
- ROS_INFO("Joint Position Controller: %s , %s",(elt->Attribute("type")),(elt->Attribute("name")));
+ ROS_DEBUG("Joint Position Controller: %s , %s",(elt->Attribute("type")),(elt->Attribute("name")));
assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPDController"));
joint_pd_controllers_.push_back(jpc);
@@ -93,7 +93,7 @@
else
{
trajectory_type_ = std::string(elt->Attribute("interpolation"));
- ROS_INFO("ArmTrajectoryController:: interpolation type:: %s",trajectory_type_.c_str());
+ ROS_DEBUG("ArmTrajectoryController:: interpolation type:: %s",trajectory_type_.c_str());
}
joint_cmd_rt_.resize(joint_pd_controllers_.size());
joint_cmd_dot_rt_.resize(joint_pd_controllers_.size());
@@ -120,7 +120,7 @@
joint_trajectory_->autocalc_timing_ = true;
- ROS_INFO("Size of trajectory points vector : %d",trajectory_points_vector.size());
+ ROS_DEBUG("Size of trajectory points vector : %d",trajectory_points_vector.size());
if(!joint_trajectory_->setTrajectory(trajectory_points_vector))
ROS_WARN("Trajectory not set correctly");
@@ -261,7 +261,7 @@
ArmTrajectoryControllerNode::ArmTrajectoryControllerNode()
: Controller(), node_(ros::Node::instance()), request_trajectory_id_(1), current_trajectory_id_(0), trajectory_wait_timeout_(10.0)
{
- std::cout<<"Controller node created"<<endl;
+ ROS_DEBUG("Controller node created");
c_ = new ArmTrajectoryController();
diagnostics_publisher_ = NULL;
}
@@ -284,7 +284,7 @@
if(topic_name_ptr_ && topic_name_.c_str())
{
- std::cout << "unsub arm controller" << topic_name_ << std::endl;
+ ROS_DEBUG("unsub arm controller %s", topic_name_.c_str());
node_->unsubscribe(topic_name_);
}
delete c_;
@@ -341,17 +341,17 @@
{
ROS_INFO("Loading ArmTrajectoryControllerNode.");
service_prefix_ = config->Attribute("name");
- ROS_INFO("The service_prefix_ is %s",service_prefix_.c_str());
+ ROS_DEBUG("The service_prefix_ is %s",service_prefix_.c_str());
double scale;
node_->param<double>(service_prefix_ + "/velocity_scaling_factor",scale,0.25);
node_->param<double>(service_prefix_ + "/trajectory_wait_timeout",trajectory_wait_timeout_,10.0);
- ROS_INFO("Trajectory wait timeout scale is %f",scale);
+ ROS_DEBUG("Trajectory wait timeout scale is %f",scale);
c_->velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
- ROS_INFO("Velocity scaling factor is %f",c_->velocity_scaling_factor_);
- ROS_INFO("Trajectory wait timeout is %f",trajectory_wait_timeout_);
+ ROS_DEBUG("Velocity scaling factor is %f",c_->velocity_scaling_factor_);
+ ROS_DEBUG("Trajectory wait timeout is %f",trajectory_wait_timeout_);
if(c_->initXml(robot, config)) // Parses subcontroller configuration
{
@@ -371,11 +371,11 @@
topic_name_= topic_name_ptr_->Attribute("name");
if(!topic_name_.c_str())
{
- std::cout<<" A listen _topic is present in the xml file but no name is specified\n";
+ ROS_ERROR(" A listen _topic is present in the xml file but no name is specified");
return false;
}
node_->subscribe(topic_name_, traj_msg_, &ArmTrajectoryControllerNode::CmdTrajectoryReceived, this, 1);
- ROS_INFO("Listening to topic: %s",topic_name_.c_str());
+ ROS_DEBUG("Listening to topic: %s",topic_name_.c_str());
}
getJointTrajectoryThresholds();
@@ -392,14 +392,14 @@
last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
- ROS_INFO("Initialized publisher");
+ ROS_DEBUG("Initialized publisher");
c_->controller_state_publisher_->msg_.name = std::string(service_prefix_);
ROS_INFO("Initialized controller");
return true;
}
- ROS_INFO("Could not initialize controller");
+ ROS_ERROR("Could not initialize controller");
return false;
}
@@ -409,7 +409,7 @@
for(int i=0; i< c_->dimension_;i++)
{
node_->param<double>(service_prefix_ + "/" + c_->joint_pd_controllers_[i]->getJointName() + "/goal_reached_threshold",c_->goal_reached_threshold_[i],GOAL_REACHED_THRESHOLD);
- ROS_INFO("Goal distance threshold for %s is %f",c_->joint_pd_controllers_[i]->getJointName().c_str(),c_->goal_reached_threshold_[i]);
+ ROS_DEBUG("Goal distance threshold for %s is %f",c_->joint_pd_controllers_[i]->getJointName().c_str(),c_->goal_reached_threshold_[i]);
}
}
@@ -467,7 +467,7 @@
void ArmTrajectoryControllerNode::CmdTrajectoryReceived()
{
- ROS_INFO("Trajectory controller:: Cmd received");
+ ROS_DEBUG("Trajectory controller:: Cmd received");
this->ros_lock_.lock();
setTrajectoryCmdFromMsg(traj_msg_);
this->ros_lock_.unlock();
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-03-20 00:06:30 UTC (rev 12719)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-03-20 00:11:39 UTC (rev 12720)
@@ -213,7 +213,7 @@
// wait for robotdesc/pr2 on param server
while(!urdf_model_.loadString(xml_content.c_str()))
{
- ROS_INFO("WARNING: base controller is waiting for robotdesc/pr2 in param server. run roslaunch send.xml or similar.");
+ ROS_WARN("base controller is waiting for robotdesc/pr2 in param server. run roslaunch send.xml or similar.");
(ros::g_node)->getParam("robotdesc/pr2",xml_content);
usleep(100000);
}
@@ -322,7 +322,7 @@
bool BaseController::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
{
- ROS_INFO("BaseController:: name: %s",config->Attribute("name"));
+ ROS_DEBUG("BaseController:: name: %s",config->Attribute("name"));
TiXmlElement *elt = config->FirstChildElement("controller");
std::vector<JointControlParam> jcp_vec;
JointControlParam jcp;
@@ -336,8 +336,8 @@
jcp.joint_name = jnt->Attribute("name");
jcp_vec.push_back(jcp);
- ROS_INFO("BaseController:: joint name: %s",jcp.joint_name.c_str());
- ROS_INFO("BaseController:: controller type: %s\n",jcp.control_type.c_str());
+ ROS_DEBUG("BaseController:: joint name: %s",jcp.joint_name.c_str());
+ ROS_DEBUG("BaseController:: controller type: %s\n",jcp.control_type.c_str());
elt = elt->NextSiblingElement("controller");
}
@@ -372,15 +372,15 @@
max_trans_vel_magnitude_ = fabs(max_vel_.x);
- ROS_INFO("BaseController:: kp_speed %f",kp_speed_);
- ROS_INFO("BaseController:: kp_caster_steer %f",caster_steer_vel_gain_);
- ROS_INFO("BaseController:: timeout %f",timeout_);
- ROS_INFO("BaseController:: max_x_dot %f",(max_vel_.x));
- ROS_INFO("BaseController:: max_y_dot %f",(max_vel_.y));
- ROS_INFO("BaseController:: max_yaw_dot %f",(max_vel_.z));
- ROS_INFO("BaseController:: max_x_accel %f",(max_accel_.x));
- ROS_INFO("BaseController:: max_y_accel %f",(max_accel_.y));
- ROS_INFO("BaseController:: max_yaw_accel %f",(max_accel_.z));
+ ROS_DEBUG("BaseController:: kp_speed %f",kp_speed_);
+ ROS_DEBUG("BaseController:: kp_caster_steer %f",caster_steer_vel_gain_);
+ ROS_DEBUG("BaseController:: timeout %f",timeout_);
+ ROS_DEBUG("BaseController:: max_x_dot %f",(max_vel_.x));
+ ROS_DEBUG("BaseController:: max_y_dot %f",(max_vel_.y));
+ ROS_DEBUG("BaseController:: max_yaw_dot %f",(max_vel_.z));
+ ROS_DEBUG("BaseController:: max_x_accel %f",(max_accel_.x));
+ ROS_DEBUG("BaseController:: max_y_accel %f",(max_accel_.y));
+ ROS_DEBUG("BaseController:: max_yaw_accel %f",(max_accel_.z));
init(jcp_vec,robot_state);
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-20 00:06:30 UTC (rev 12719)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-03-20 00:11:39 UTC (rev 12720)
@@ -579,17 +579,15 @@
}
catch(tf::LookupException& ex)
{
- puts("no odom->base Tx yet");
- ROS_DEBUG("%s\n", ex.what());
+ ROS_DEBUG("No odom->base Tx yet: %s", ex.what());
}
catch(tf::ConnectivityException& ex)
{
- puts("no odom->base Tx yet");
- ROS_DEBUG("%s\n", ex.what());
+ ROS_DEBUG("No odom->base Tx yet: %s\n", ex.what());
}
catch(tf::ExtrapolationException& ex)
{
- puts("Extrapolation exception");
+ ROS_DEBUG("Extrapolation exception");
}
base_odom_.unlock();
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-03-20 00:06:30 UTC (rev 12719)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-03-20 00:11:39 UTC (rev 12720)
@@ -377,7 +377,7 @@
clock_gettime(CLOCK_REALTIME, &init_end);
double duration = 1.0e3 * (init_end.tv_sec - init_start.tv_sec) +
double(init_end.tv_nsec)/1.0e6 - double(init_start.tv_nsec)/1.0e6;
- ROS_INFO(" Initialized %s in %.3lf ms", add_reqs[i].name.c_str(), duration);
+ ROS_DEBUG(" Initialized %s in %.3lf ms", add_reqs[i].name.c_str(), duration);
if (!initialized)
{
delete c;
@@ -419,7 +419,7 @@
double duration = 1.0e3 * (end_time.tv_sec - start_time.tv_sec) +
double(end_time.tv_nsec)/1.0e6 - double(start_time.tv_nsec)/1.0e6;
- ROS_INFO("Controller replacement took %.3lf ms", duration);
+ ROS_DEBUG("Controller replacement took %.3lf ms", duration);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-03-20 20:48:49
|
Revision: 12771
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12771&view=rev
Author: stuglaser
Date: 2009-03-20 20:48:31 +0000 (Fri, 20 Mar 2009)
Log Message:
-----------
Silenced urdf errors on robot bringup
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_model/src/link.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2.xacro.xml
Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp 2009-03-20 20:22:51 UTC (rev 12770)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp 2009-03-20 20:48:31 UTC (rev 12771)
@@ -167,7 +167,7 @@
visual_.reset(new Visual);
if (!visual_->initXml(v))
{
- ROS_ERROR("Could not parse visual element for link \"%s\"", name_.c_str());
+ ROS_DEBUG/*ERROR*/("Could not parse visual element for link \"%s\"", name_.c_str());
visual_.reset();
}
}
@@ -253,7 +253,7 @@
geometry_.reset(parseGeometry(geom));
if (!geometry_)
{
- ROS_ERROR("Malformed geometry for Visual element");
+ ROS_DEBUG/*ERROR*/("Malformed geometry for Visual element");
return false;
}
@@ -339,7 +339,7 @@
{
if (!c->Attribute("filename"))
{
- ROS_ERROR("Mesh must contain a filename attribute");
+ ROS_DEBUG/*ERROR*/("Mesh must contain a filename attribute");
return false;
}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2.xacro.xml 2009-03-20 20:22:51 UTC (rev 12770)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2.xacro.xml 2009-03-20 20:48:31 UTC (rev 12771)
@@ -115,7 +115,7 @@
</visual>
<collision>
- <origin xyz="0 0 0" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
<geometry name="high_def_optical_collision">
<box size="0.01 0.01 0.01" />
</geometry>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-03-20 21:59:27
|
Revision: 12781
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12781&view=rev
Author: veedee
Date: 2009-03-20 21:59:13 +0000 (Fri, 20 Mar 2009)
Log Message:
-----------
cleaner door message
Modified Paths:
--------------
pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/prcore/robot_msgs/msg/Door.msg
Modified: pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp 2009-03-20 21:53:11 UTC (rev 12780)
+++ pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp 2009-03-20 21:59:13 UTC (rev 12781)
@@ -470,7 +470,7 @@
}
- resp.door.door_boundary = polygon_transformed;
+ //resp.door.door_boundary = polygon_transformed; -> gone from Door.msg
duration = ros::Time::now () - ts;
ROS_INFO ("Door found. Result in frame %s \n P1 = [%f, %f, %f]. P2 = [%f, %f, %f]. \n Height = %f. \n Handle = [%f, %f, %f]. \n Total time: %f.",
Modified: pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-03-20 21:53:11 UTC (rev 12780)
+++ pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-03-20 21:59:13 UTC (rev 12781)
@@ -510,20 +510,13 @@
resp.doors[nr_d].door_p1 = min_p;
resp.doors[nr_d].door_p2 = max_p;
- resp.doors[nr_d].door_boundary = pmap_.polygons[cc];
resp.doors[nr_d].normal.x = coeff[cc][0];
resp.doors[nr_d].normal.y = coeff[cc][1];
resp.doors[nr_d].normal.z = coeff[cc][2];
- resp.doors[nr_d].plane_d = coeff[cc][3];
// Need min/max Z
cloud_geometry::statistics::getMinMax (&pmap_.polygons[cc], min_p, max_p);
resp.doors[nr_d].height = fabs (max_p.z - min_p.z);
- resp.doors[nr_d].width = sqrt ((resp.doors[nr_d].door_p1.x - resp.doors[nr_d].door_p2.x) *
- (resp.doors[nr_d].door_p1.x - resp.doors[nr_d].door_p2.x) +
- (resp.doors[nr_d].door_p1.y - resp.doors[nr_d].door_p2.y) *
- (resp.doors[nr_d].door_p1.y - resp.doors[nr_d].door_p2.y)
- );
nr_d++;
}
@@ -545,7 +538,11 @@
{
ROS_INFO (" %d -> P1 = [%g, %g, %g]. P2 = [%g, %g, %g]. Width = %g. Height = %g. Weight = %g.", cd,
resp.doors[cd].door_p1.x, resp.doors[cd].door_p1.y, resp.doors[cd].door_p1.z, resp.doors[cd].door_p2.x, resp.doors[cd].door_p2.y, resp.doors[cd].door_p2.z,
- resp.doors[cd].width, resp.doors[cd].height, resp.doors[cd].weight);
+ sqrt ((resp.doors[nr_d].door_p1.x - resp.doors[nr_d].door_p2.x) *
+ (resp.doors[nr_d].door_p1.x - resp.doors[nr_d].door_p2.x) +
+ (resp.doors[nr_d].door_p1.y - resp.doors[nr_d].door_p2.y) *
+ (resp.doors[nr_d].door_p1.y - resp.doors[nr_d].door_p2.y)
+ ), resp.doors[cd].height, resp.doors[cd].weight);
}
ROS_INFO (" Total time: %g.", duration.toSec ());
Modified: pkg/trunk/prcore/robot_msgs/msg/Door.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Door.msg 2009-03-20 21:53:11 UTC (rev 12780)
+++ pkg/trunk/prcore/robot_msgs/msg/Door.msg 2009-03-20 21:59:13 UTC (rev 12781)
@@ -3,16 +3,13 @@
robot_msgs/Point32 frame_p2 ## position of the door frame
robot_msgs/Point32 door_p1 ## Ground plane projection of a point on the plane of the door
robot_msgs/Point32 door_p2 ## Ground plane projection of a point on the plane of the door
-robot_msgs/Point32 handle ## Position of the door handle w.r.t a frame defined at the axis of the door and on the ground plane with Z axis pointing up and X axis along the door
+robot_msgs/Point32 handle ## Position of the door handle
-float32 width
float32 height
float32 weight ## Given some perception algorithm, how sure are we that this is a good candidate
int32 hinge ## is the hinge at frame_p1 (=1), frame_p2 (=2) or unknown (=-1)
int32 rot_dir ## does the door rotate open clockwise (=1), counterclockwise (=2), or unknown (=-1)
-Polygon3D door_boundary
+Vector3 normal ## <a,b,c> plane coefficients, i.e., the door plane normal
-Vector3 normal ## <a,b,c> plane coefficients
-float32 plane_d ## <d> plane coefficient
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-03-25 23:51:47
|
Revision: 12988
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12988&view=rev
Author: tfoote
Date: 2009-03-25 23:51:37 +0000 (Wed, 25 Mar 2009)
Log Message:
-----------
making it easier to configure from a parameter
Modified Paths:
--------------
pkg/trunk/prcore/filters/include/filters/filter_chain.h
pkg/trunk/util/laser_scan/src/generic_laser_filter_node.cpp
Modified: pkg/trunk/prcore/filters/include/filters/filter_chain.h
===================================================================
--- pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-03-25 22:36:23 UTC (rev 12987)
+++ pkg/trunk/prcore/filters/include/filters/filter_chain.h 2009-03-25 23:51:37 UTC (rev 12988)
@@ -104,7 +104,6 @@
}
-
/********************** Do the allocation *********************/
buffer0_.resize(size);
buffer1_.resize(size);
@@ -153,7 +152,19 @@
return result;
};
+ /**@brief Configure the filter from xml stored on the parameter server
+ * This is simply a convienience function for this is the recommended way to setup filters*/
+ bool configureFromXMLString(unsigned int size, std::string filter_xml)
+ {
+ TiXmlDocument xml_doc;
+ xml_doc.Parse(filter_xml.c_str());
+ TiXmlElement * config = xml_doc.RootElement();
+
+ return this->configure(1, config);
+ };
+
+
/** \brief Clear all filters from this chain */
bool clear()
{
Modified: pkg/trunk/util/laser_scan/src/generic_laser_filter_node.cpp
===================================================================
--- pkg/trunk/util/laser_scan/src/generic_laser_filter_node.cpp 2009-03-25 22:36:23 UTC (rev 12987)
+++ pkg/trunk/util/laser_scan/src/generic_laser_filter_node.cpp 2009-03-25 23:51:37 UTC (rev 12988)
@@ -44,15 +44,13 @@
MedianFilterNode(ros::Node& anode) : filter_chain_(), node_(anode)
{
- std::string filter_xml;
node_.advertise<laser_scan::LaserScan>("~output", 1000);
- node_.param("~filters", filter_xml, median_filter_xml);
- ROS_INFO("Got ~filters as: %s\n", filter_xml.c_str());
- TiXmlDocument xml_doc;
- xml_doc.Parse(filter_xml.c_str());
- TiXmlElement * config = xml_doc.RootElement();
- filter_chain_.configure(1, config);
+ std::string filter_xml;
+ node_.param("~filters", filter_xml, std::string("<filters><!--Filter Parameter Not Set--></filters>"));
+ ROS_INFO("Got parameter'~filters' as: %s\n", filter_xml.c_str());
+
+ filter_chain_.configureFromXMLString(1, filter_xml);
node_.subscribe("scan_in", msg, &MedianFilterNode::callback,this, 3);
}
void callback()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-03-26 00:33:40
|
Revision: 12994
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12994&view=rev
Author: mariusmuja
Date: 2009-03-26 00:33:27 +0000 (Thu, 26 Mar 2009)
Log Message:
-----------
Added some debugging code to door_stereo_detector
Modified Paths:
--------------
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-03-26 00:31:18 UTC (rev 12993)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-03-26 00:33:27 UTC (rev 12994)
@@ -98,12 +98,7 @@
tf::MessageNotifier<robot_msgs::PointCloud>* message_notifier_;
- robot_msgs::Door door_msg_;
- robot_msgs::Door door_msg_in_;
-
- boost::mutex door_msg_mutex_ ;
-
/********** Parameters that need to be gotten from the param server *******/
std::string door_msg_topic_, stereo_cloud_topic_;
int sac_min_points_per_model_;
@@ -147,8 +142,6 @@
node_->param("~p_door_rot_dir" , tmp2, -1); door_msg_.rot_dir = tmp2;
door_msg_.header.frame_id = "base_footprint";
*/
-// node_->subscribe(stereo_cloud_topic_,cloud_, &DoorStereo::stereoCloudCallBack,this,1);
- node_->subscribe(door_msg_topic_,door_msg_in_, &DoorStereo::doorMsgCallBack,this,1);
node_->advertise<robot_msgs::VisualizationMarker>( "visualizationMarker", 0 );
message_notifier_ = new tf::MessageNotifier<robot_msgs::PointCloud> (tf_, node_, boost::bind(&DoorStereo::stereoCloudCallBack, this, _1), stereo_cloud_topic_.c_str (), "stereo_link", 1);
@@ -156,7 +149,6 @@
~DoorStereo()
{
- node_->unsubscribe(door_msg_topic_,&DoorStereo::doorMsgCallBack,this);
node_->unadvertise("visualizationMarker");
delete message_notifier_;
}
@@ -228,15 +220,6 @@
}
}
-
- void doorMsgCallBack()
- {
- door_msg_mutex_.lock();
- door_msg_ = door_msg_in_;
- door_msg_mutex_.unlock();
- //populate the door message
- }
-
void fitSACLines(PointCloud *points, vector<int> indices, vector<vector<int> > &inliers, vector<vector<double> > &coeff, vector<Point32> &line_segment_min, vector<Point32> &line_segment_max)
{
Point32 minP, maxP;
@@ -268,6 +251,42 @@
ROS_INFO("Transformed axis: %f %f %f",axis_point_32.x,axis_point_32.y,axis_point_32.z);
+
+ // visualize the axis
+ {
+ robot_msgs::VisualizationMarker marker;
+ marker.header.frame_id = cloud_.header.frame_id;
+ marker.header.stamp = ros::Time((uint64_t)0ULL);
+ marker.id = 100001;
+ marker.type = robot_msgs::VisualizationMarker::LINE_STRIP;
+ marker.action = robot_msgs::VisualizationMarker::ADD;
+ marker.x = 0.0;
+ marker.y = 0.0;
+ marker.z = 0.0;
+ marker.yaw = 0.0;
+ marker.pitch = 0.0;
+ marker.roll = 0.0;
+ marker.xScale = 0.01;
+ marker.yScale = 0.1;
+ marker.zScale = 0.1;
+ marker.alpha = 255;
+ marker.r = 0;
+ marker.g = 255;
+ marker.b = 0;
+ marker.set_points_size(2);
+
+ marker.points[0].x = 0;
+ marker.points[0].y = 0;
+ marker.points[0].z = 0;
+
+ marker.points[1].x = axis_transformed.vector.x;
+ marker.points[1].y = axis_transformed.vector.y;
+ marker.points[1].z = axis_transformed.vector.z;
+
+ node_->publish( "visualizationMarker", marker );
+ }
+
+
model->setAxis (&axis_point_32);
model->setEpsAngle (eps_angle_);
Modified: pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp
===================================================================
--- pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-03-26 00:31:18 UTC (rev 12993)
+++ pkg/trunk/vision/door_handle_detect/src/door_handle_detect.cpp 2009-03-26 00:33:27 UTC (rev 12994)
@@ -654,6 +654,7 @@
tf_->transformPointCloud("base_link", cloud, base_cloud);
}
catch(tf::ExtrapolationException& ex) {
+ tf_->clear();
ROS_WARN("TF exception: %s", ex.what());
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-03-26 01:06:47
|
Revision: 12996
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12996&view=rev
Author: mariusmuja
Date: 2009-03-26 01:06:40 +0000 (Thu, 26 Mar 2009)
Log Message:
-----------
Outlet spotting publishes a BoundingBoxStamped message now.
Modified Paths:
--------------
pkg/trunk/vision/outlet_spotting/detect.launch
pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/BoundingBoxStamped.msg
Added: pkg/trunk/prcore/robot_msgs/msg/BoundingBoxStamped.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/BoundingBoxStamped.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/BoundingBoxStamped.msg 2009-03-26 01:06:40 UTC (rev 12996)
@@ -0,0 +1,3 @@
+Header header
+robot_msgs/Point32 center
+robot_msgs/Point32 extents
\ No newline at end of file
Modified: pkg/trunk/vision/outlet_spotting/detect.launch
===================================================================
--- pkg/trunk/vision/outlet_spotting/detect.launch 2009-03-26 01:00:53 UTC (rev 12995)
+++ pkg/trunk/vision/outlet_spotting/detect.launch 2009-03-26 01:06:40 UTC (rev 12996)
@@ -23,6 +23,8 @@
<node pkg="dcam" type="stereodcam" respawn="false"/>
<node pkg="stereo_image_proc" type="stereoproc" respawn="false"/>
- <node pkg="outlet_spotting" name="outlet_spotting" type="outlet_spotting" respawn="false" output="screen"/>
+ <node pkg="outlet_spotting" name="outlet_spotting" type="outlet_spotting" respawn="false" output="screen">
+ <param name="display" type="bool" value="True"/>
+ </node>
</launch>
Modified: pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-03-26 01:00:53 UTC (rev 12995)
+++ pkg/trunk/vision/outlet_spotting/src/outlet_spotting.cpp 2009-03-26 01:06:40 UTC (rev 12996)
@@ -55,7 +55,7 @@
#include "image_msgs/Image.h"
#include "robot_msgs/PointCloud.h"
#include "robot_msgs/Point32.h"
-#include "robot_msgs/OrientedBoundingBox.h"
+#include "robot_msgs/BoundingBoxStamped.h"
#include "std_msgs/UInt8.h" //for projector status
@@ -89,7 +89,7 @@
robot_msgs::PointCloud cloud;
- robot_msgs::OrientedBoundingBox outlet_bbox;
+ robot_msgs::BoundingBoxStamped outlet_bbox;
IplImage* left;
// IplImage* right;''
@@ -142,7 +142,7 @@
sync.ready();
- advertise<OrientedBoundingBox>("stereo/outlet_bbox",1);
+ advertise<BoundingBoxStamped>("stereo/outlet_bbox",1);
}
~OutletSpotting()
@@ -547,6 +547,10 @@
// }
+ //publish bounding_box
+ outlet_bbox.header.frame_id = cloud.header.frame_id;
+ outlet_bbox.header.stamp = cloud.header.stamp;
+
outlet_bbox.center.x = (bb_min.x+bb_max.x)/2;
outlet_bbox.center.y = (bb_min.y+bb_max.y)/2;
outlet_bbox.center.z = (bb_min.z+bb_max.z)/2;
@@ -555,8 +559,6 @@
outlet_bbox.extents.y = (bb_max.y-bb_min.y)/2;
outlet_bbox.extents.z = (bb_max.z-bb_min.z)/2;
- outlet_bbox.angle = 0;
-
publish("stereo/outlet_bbox", outlet_bbox);
return true;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rph...@us...> - 2009-03-26 02:26:55
|
Revision: 13000
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13000&view=rev
Author: rphilippsen
Date: 2009-03-26 02:26:50 +0000 (Thu, 26 Mar 2009)
Log Message:
-----------
Support more cost thresholds in XYTHETALAT planning.
* new: setting the "cost_inscribed" and "cost_possibly_circumscribed"
parameters if SBPL's EnvironmentNAVXYTHETALAT
* new: libsunflower now allows us to compute the cost at the circumscribed
radius, updated mpglue wrappers accordingly
* fix: plotting of the non-lethal but inscribed region was broken
* fix: mpbench wrongly initialized cost "fade-out" to less than
inflation radius
* fix: dump of command-line args was missing a space
Modified Paths:
--------------
pkg/trunk/3rdparty/libsunflower/Makefile
pkg/trunk/motion_planning/mpbench/mkhtml.py
pkg/trunk/motion_planning/mpbench/src/gfx.cpp
pkg/trunk/motion_planning/mpbench/src/world.cpp
pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h
pkg/trunk/motion_planning/mpglue/src/costmap.cpp
pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
Modified: pkg/trunk/3rdparty/libsunflower/Makefile
===================================================================
--- pkg/trunk/3rdparty/libsunflower/Makefile 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/3rdparty/libsunflower/Makefile 2009-03-26 02:26:50 UTC (rev 13000)
@@ -19,7 +19,7 @@
all: installed
TARBALL_VERSION= r910
-SVN_REVISION= -r 967
+SVN_REVISION= -r 971
TARBALL= build/libsunflower-$(TARBALL_VERSION).tar.gz
TARBALL_URL= http://pr.willowgarage.com/downloads/libsunflower-$(TARBALL_VERSION).tar.gz
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.py
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.py 2009-03-26 02:26:50 UTC (rev 13000)
@@ -82,7 +82,7 @@
print >>out, ' <td>mpbenchmark failed with code %d, see <a href="%s">console output</a></td>' % (retcode, conslogname)
else:
print >>out, ' <td><table border="0" cellpadding="1">'
- print >>out, ' <tr><td colspan="3">./mpbenchmark' + args + '</td></tr>'
+ print >>out, ' <tr><td colspan="3">./mpbenchmark ' + args + '</td></tr>'
print >>out, ' <tr>'
print >>out, ' <td><a href="' + basename + '.txt">log</a></td>'
print >>out, ' <td><a href="' + conslogname + '">console</a></td>'
Modified: pkg/trunk/motion_planning/mpbench/src/gfx.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/src/gfx.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -198,7 +198,7 @@
virtual double GetTheta() const { return 0; }
virtual double GetDelta() const { return gframe.Delta(); }
virtual sfl::GridFrame const * GetGridFrame() { return &gframe; }
- virtual int GetObstacle() const { return costmap->getLethalCost(); }
+ virtual int GetObstacle() const { return costmap->getInscribedCost(); }
virtual int GetFreespace() const { return 0; }
virtual ssize_t GetXBegin() const { return costmap->getXBegin(); }
virtual ssize_t GetXEnd() const { return costmap->getXEnd(); }
Modified: pkg/trunk/motion_planning/mpbench/src/world.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/world.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpbench/src/world.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -240,7 +240,7 @@
<< " (currently hardcoded to sfl::Mapper2d)\n";
boost::shared_ptr<sfl::Mapper2d::travmap_grow_strategy>
growstrategy(new sfl::Mapper2d::always_grow());
- double const buffer_zone(opt_.costmap_circumscribed_radius - opt_.costmap_inscribed_radius);
+ double const buffer_zone(opt_.costmap_inflation_radius - opt_.costmap_inscribed_radius);
double const padding_factor(0);
shared_ptr<sfl::Mapper2d> m2d(new sfl::Mapper2d(gridframe_,
0, 0, // grid_xbegin, grid_xend
@@ -252,12 +252,13 @@
opt_.costmap_obstacle_cost,
// costmap_2d seems to use
// a quadratic decay in r7215
- sfl::exponential_travmap_cost_decay(2),
+ shared_ptr<sfl::exponential_travmap_cost_decay>(new sfl::exponential_travmap_cost_decay(2)),
"m2d",
sfl::RWlock::Create("m2d"),
growstrategy));
- cm = mpglue::createCostmapper(m2d);
+ cm = mpglue::createCostmapper(m2d, m2d->ComputeCost(opt_.costmap_circumscribed_radius));
+
return cm;
}
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/costmap.h 2009-03-26 02:26:50 UTC (rev 13000)
@@ -138,10 +138,15 @@
CostmapAccessor * createCostmapAccessor(costmap_2d::ObstacleMapAccessor const * cm);
- CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt);
- CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt);
+
+ CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost);
+
+ CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt,
+ int possibly_circumscribed_cost);
IndexTransform * createIndexTransform(costmap_2d::ObstacleMapAccessor const * cm);
+
IndexTransform * createIndexTransform(sfl::GridFrame const * gf);
}
Modified: pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/include/mpglue/costmapper.h 2009-03-26 02:26:50 UTC (rev 13000)
@@ -124,7 +124,8 @@
};
- boost::shared_ptr<Costmapper> createCostmapper(boost::shared_ptr<sfl::Mapper2d> m2d);
+ boost::shared_ptr<Costmapper> createCostmapper(boost::shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost);
}
Modified: pkg/trunk/motion_planning/mpglue/src/costmap.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/costmap.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -130,22 +130,20 @@
sfl::RDTravmap const * rdt_;
int const wobstCost_;
int const cobstCost_;
- int const cobstCostMinusOne_;
+ int const possibly_circumscribed_cost_;
- sflRDTAccessor(sfl::RDTravmap const * rdt)
+ sflRDTAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost)
: rdt_(rdt),
// Beware: W-space means "non-inflated", which is tagged by
// making the cost higher than necessary in sfl::Mapper2d.
wobstCost_(rdt->GetObstacle() + 1),
cobstCost_(rdt->GetObstacle()),
- // This is a bit of a hack to work around the fact that
- // sfl::Mapper2d does not distinguish between inscribed and
- // circumscribed robot radii.
- cobstCostMinusOne_(rdt->GetObstacle() - 1) {}
+ possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual mpglue::cost_t getLethalCost() const { return wobstCost_; }
virtual mpglue::cost_t getInscribedCost() const { return cobstCost_; }
- virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return cobstCostMinusOne_; }
+ virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return possibly_circumscribed_cost_; }
virtual ssize_t getXBegin() const { return rdt_->GetXBegin(); }
virtual ssize_t getXEnd() const { return rdt_->GetXEnd(); }
@@ -171,20 +169,9 @@
virtual bool isPossiblyCircumcribed(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
- // Problem: sfl::Mapper2d() does not distinguish between
- // inscribed and circumscribed radii, it simply assumes the
- // whole robot fits in the radius, period. On the one hand
- // that's like saying the inscribed radius is the same as the
- // circumscribed radius, but given that we tend to initialize
- // sfl::Mapper2d with the inscribed radius as "the" radius, that
- // does not hold.
- //
- // XXXX broken hack...another point to discuss. For the moment
- // treat it as if obstacle-1 means "between inscribed and
- // circumscribed" in the sfl::Mapper2d case.
int value;
if (rdt_->GetValue(index_x, index_y, value))
- return value >= cobstCostMinusOne_;
+ return value >= possibly_circumscribed_cost_;
return out_of_bounds_reply;
}
@@ -205,15 +192,16 @@
sfl::TraversabilityMap const * travmap_;
int const wobstCost_;
int const cobstCost_;
- int const cobstCostMinusOne_;
+ int const possibly_circumscribed_cost_;
- sflTravmapAccessor(sfl::TraversabilityMap const * travmap)
+ sflTravmapAccessor(sfl::TraversabilityMap const * travmap,
+ int possibly_circumscribed_cost)
: travmap_(travmap), wobstCost_(travmap->obstacle + 1), cobstCost_(travmap->obstacle),
- cobstCostMinusOne_(travmap->obstacle - 1) {}
+ possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual mpglue::cost_t getLethalCost() const { return wobstCost_; }
virtual mpglue::cost_t getInscribedCost() const { return cobstCost_; }
- virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return cobstCostMinusOne_; }
+ virtual mpglue::cost_t getPossiblyCircumcribedCost() const { return possibly_circumscribed_cost_; }
virtual ssize_t getXBegin() const { return travmap_->grid.xbegin(); }
virtual ssize_t getXEnd() const { return travmap_->grid.xend(); }
@@ -239,11 +227,9 @@
virtual bool isPossiblyCircumcribed(ssize_t index_x, ssize_t index_y,
bool out_of_bounds_reply) const {
- // Problem (again): sfl::Mapper2d() does not distinguish between
- // inscribed and circumscribed radii...
int value;
if (travmap_->GetValue(index_x, index_y, value))
- return value >= cobstCostMinusOne_;
+ return value >= possibly_circumscribed_cost_;
return out_of_bounds_reply;
}
@@ -333,11 +319,13 @@
CostmapAccessor * createCostmapAccessor(costmap_2d::ObstacleMapAccessor const * cm)
{ return new cm2dCostmapAccessor(cm); }
- CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt)
- { return new sflRDTAccessor(rdt); }
+ CostmapAccessor * createCostmapAccessor(sfl::RDTravmap const * rdt,
+ int possibly_circumscribed_cost)
+ { return new sflRDTAccessor(rdt, possibly_circumscribed_cost); }
- CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt)
- { return new sflTravmapAccessor(rdt); }
+ CostmapAccessor * createCostmapAccessor(sfl::TraversabilityMap const * rdt,
+ int possibly_circumscribed_cost)
+ { return new sflTravmapAccessor(rdt, possibly_circumscribed_cost); }
IndexTransform * createIndexTransform(costmap_2d::ObstacleMapAccessor const * cm)
{ return new cm2dTransform(cm); }
Modified: pkg/trunk/motion_planning/mpglue/src/costmapper.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/costmapper.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -47,14 +47,17 @@
mutable shared_ptr<sfl::RDTravmap> rdt_;
mutable shared_ptr<mpglue::CostmapAccessor> cma_;
mutable shared_ptr<mpglue::IndexTransform> idxt_;
+ int const possibly_circumscribed_cost_;
- sflCostmapper(shared_ptr<sfl::Mapper2d> m2d): m2d_(m2d) {}
+ sflCostmapper(shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost)
+ : m2d_(m2d), possibly_circumscribed_cost_(possibly_circumscribed_cost) {}
virtual boost::shared_ptr<mpglue::CostmapAccessor const> getAccessor() const
{
if ( ! cma_) {
rdt_ = m2d_->CreateRDTravmap();
- cma_.reset(mpglue::createCostmapAccessor(rdt_.get()));
+ cma_.reset(mpglue::createCostmapAccessor(rdt_.get(), possibly_circumscribed_cost_));
}
return cma_;
}
@@ -105,9 +108,10 @@
namespace mpglue {
- shared_ptr<Costmapper> createCostmapper(shared_ptr<sfl::Mapper2d> m2d)
+ shared_ptr<Costmapper> createCostmapper(shared_ptr<sfl::Mapper2d> m2d,
+ int possibly_circumscribed_cost)
{
- shared_ptr<Costmapper> foo(new sflCostmapper(m2d));
+ shared_ptr<Costmapper> foo(new sflCostmapper(m2d, possibly_circumscribed_cost));
return foo;
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-03-26 01:30:18 UTC (rev 12999)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-03-26 02:26:50 UTC (rev 13000)
@@ -35,6 +35,7 @@
#include <mpglue/sbpl_environment.h>
#include <costmap_2d/costmap_2d.h>
#include <sbpl/headers.h>
+#include <sfl/util/strutil.hpp>
using namespace mpglue;
using namespace std;
@@ -395,6 +396,18 @@
std::ostream * dbgos) throw(std::exception)
{
EnvironmentNAVXYTHETALAT * env(new EnvironmentNAVXYTHETALAT());
+ if ( ! env->SetEnvParameter("cost_inscribed", cm->getInscribedCost())) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
+ " EnvironmentNAVXYTHETALAT::SetEnvParameter(\"cost_inscribed\", "
+ + sfl::to_string(cm->getInscribedCost()) + ") failed");
+ }
+ if ( ! env->SetEnvParameter("cost_possibly_circumscribed", cm->getPossiblyCircumcribedCost())) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::createXYThetaLattice():"
+ " EnvironmentNAVXYTHETALAT::SetEnvParameter(\"cost_possibly_circumscribed\", "
+ + sfl::to_string(cm->getPossiblyCircumcribedCost()) + ") failed");
+ }
int const obst_cost_thresh(cm->getLethalCost());
vector<sbpl_2Dpt_t> perimeterptsV;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-03-27 01:49:13
|
Revision: 13027
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13027&view=rev
Author: gerkey
Date: 2009-03-27 01:49:03 +0000 (Fri, 27 Mar 2009)
Log Message:
-----------
Moved rosstage into stage and renamed it stageros. Updated all the launch
files I could identify.
Modified Paths:
--------------
pkg/trunk/3rdparty/stage/manifest.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/sim.launch
pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/demos/tabletop_manipulation/scripts/movebase.py
pkg/trunk/highlevel/test_executive_trex_pr2/cfg/launch_stage.xml
pkg/trunk/highlevel/test_executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/test_highlevel_controllers/manifest.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/build_for_stage.sh
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_2.5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_stage_amcl_5cm.xml
pkg/trunk/highlevel/test_highlevel_controllers/test/testcase.xml.in
Added Paths:
-----------
pkg/trunk/3rdparty/stage/CMakeLists.txt
pkg/trunk/3rdparty/stage/Makefile.stage
pkg/trunk/3rdparty/stage/src/
pkg/trunk/3rdparty/stage/src/stageros.cpp
pkg/trunk/3rdparty/stage/test/
pkg/trunk/3rdparty/stage/world/
Removed Paths:
-------------
pkg/trunk/3rdparty/stage/Makefile
pkg/trunk/simulators/rosstage/
Copied: pkg/trunk/3rdparty/stage/CMakeLists.txt (from rev 13026, pkg/trunk/simulators/rosstage/CMakeLists.txt)
===================================================================
--- pkg/trunk/3rdparty/stage/CMakeLists.txt (rev 0)
+++ pkg/trunk/3rdparty/stage/CMakeLists.txt 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+include(FindPkgConfig)
+rospack(stage)
+
+
+add_custom_target(build_stage ALL
+ COMMAND cmake -E chdir ${PROJECT_SOURCE_DIR} make -f Makefile.stage)
+set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES stage-svn)
+
+rospack_add_boost_directories()
+
+pkg_check_modules(STAGE REQUIRED stage)
+include_directories(${STAGE_INCLUDE_DIRS})
+link_directories(${STAGE_LIBRARY_DIRS})
+rospack_add_executable(bin/stageros src/stageros.cpp)
+rospack_link_boost(bin/stageros thread)
+add_dependencies(bin/stageros build_stage)
+
+set(ENV{PKG_CONFIG_PATH} "${PROJECT_SOURCE_DIR}/stage/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}")
+rospack_add_compile_flags(bin/stageros ${STAGE_CFLAGS_OTHERS})
+target_link_libraries(bin/stageros ${STAGE_LIBRARIES})
+rospack_add_link_flags(bin/stageros ${STAGE_LDFLAGS_OTHERS})
+
+
+rospack_add_rostest(test/hztest.xml)
Property changes on: pkg/trunk/3rdparty/stage/CMakeLists.txt
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/3rdparty/stage/Makefile
===================================================================
--- pkg/trunk/3rdparty/stage/Makefile 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/3rdparty/stage/Makefile 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,19 +0,0 @@
-all: installed
-
-SVN_DIR = stage-svn
-SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros
-SVN_REVISION = -r 7126
-include $(shell rospack find mk)/svn_checkout.mk
-
-installed: $(SVN_DIR) patched
- cd $(SVN_DIR) && autoreconf -i -s
- cd $(SVN_DIR) && ./configure --prefix=$(PWD)/stage
- cd $(SVN_DIR) && make install
- touch installed
-
-clean:
- -cd $(SVN_DIR) && make clean
- rm -rf stage installed
-
-wipe: clean
- rm -rf $(SVN_DIR)
Copied: pkg/trunk/3rdparty/stage/Makefile.stage (from rev 13026, pkg/trunk/3rdparty/stage/Makefile)
===================================================================
--- pkg/trunk/3rdparty/stage/Makefile.stage (rev 0)
+++ pkg/trunk/3rdparty/stage/Makefile.stage 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,19 @@
+all: installed
+
+SVN_DIR = stage-svn
+SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros
+SVN_REVISION = -r 7126
+include $(shell rospack find mk)/svn_checkout.mk
+
+installed: $(SVN_DIR) patched Makefile.stage
+ cd $(SVN_DIR) && autoreconf -i -s
+ cd $(SVN_DIR) && ./configure --prefix=`pwd`/../stage
+ cd $(SVN_DIR) && make install
+ touch installed
+
+clean:
+ -cd $(SVN_DIR) && make clean
+ rm -rf stage installed patched
+
+wipe: clean
+ rm -rf $(SVN_DIR)
Property changes on: pkg/trunk/3rdparty/stage/Makefile.stage
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/3rdparty/stage/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/stage/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/3rdparty/stage/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,22 +1,27 @@
<package>
<description brief="Stage">
-This package contains Stage, from the Player Project
+This package contains the Stage robot simulator, from the Player Project
(http://playerstage.sf.net). This package does not modify Stage in any
-way; it simply provides a convenient way to download and compile the
-headers and libraries in a way that can be managed by the ROS dependency
-system.
+way. This package also provides stageros, a ROS node that uses Stage.
</description>
<author>Richard Vaughan, with contributions from many others. See web page for a full credits list.</author>
<license>GPL</license>
-<review status="3rdparty" notes=""/>
+<review status="unreviewed" notes=""/>
<url>http://playerstage.sf.net</url>
<export>
<cpp lflags="-Wl,-rpath,${prefix}/stage/lib `PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs stage`" cflags="`PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags stage`"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/stage-cvs"/>
</export>
<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros"/>
+
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
+ <depend package="laser_scan" />
+ <depend package="tf" />
+
<sysdepend os="ubuntu" version="8.04-hardy" package="autoconf"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="automake"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libtool"/>
Copied: pkg/trunk/3rdparty/stage/src/stageros.cpp (from rev 13026, pkg/trunk/simulators/rosstage/rosstage.cc)
===================================================================
--- pkg/trunk/3rdparty/stage/src/stageros.cpp (rev 0)
+++ pkg/trunk/3rdparty/stage/src/stageros.cpp 2009-03-27 01:49:03 UTC (rev 13027)
@@ -0,0 +1,418 @@
+/*
+ * stageros
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+@b stageros wraps the Stage 2-D multi-robot simulator, via @b libstage.
+
+For detailed documentation,
+consult the <a href="http://playerstage.sourceforge.net/doc/stage-cvs">Stage manual</a>.
+
+This node finds the first Stage model of type @b laser, and the first model
+of type @b position, and maps these models to the ROS topics given below.
+If a laser and a position model are not found, stageros exits.
+
+@todo Define a more general method for mapping Stage models onto ROS topics
+/ services. Something like the Player/Stage model, in which a Player .cfg
+file is used to map named Stage models onto Player devices, is probably the
+way to go. The same technique can be used for rosgazebo.
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ stageros <world> [standard ROS args]
+@endverbatim
+
+@param world The Stage .world file to load.
+
+@par Example
+
+@verbatim
+$ stageros willow-erratic.world
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+If there is only one position model defined in the world file, all of these
+topics appear at the top namespace. However, if >1 position models exist,
+these topics are pushed down into their own namespaces, by prefixing the
+topics with "robot_<i>/" , e.g., "robot_0/cmd_vel", etc.
+
+Subscribes to (name/type):
+- @b "cmd_vel"/PoseDot : velocity commands to differentially drive the
+ position model of the robot
+
+Publishes to (name / type):
+- @b "odom"/RobotBase2DOdom : odometry data from the position model.
+- @b "base_scan"/LaserScan : scans from the laser model
+- @b "base_pose_ground_truth"/PoseWithRatesStamped : ground truth pos
+
+<hr>
+
+@section parameters ROS parameters
+
+- None
+
+
+ **/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// libstage
+#include <stage.hh>
+
+// roscpp
+#include <ros/node.h>
+#include "boost/thread/mutex.hpp"
+#include <laser_scan/LaserScan.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <robot_msgs/PoseWithRatesStamped.h>
+#include <robot_msgs/PoseDot.h>
+#include <roslib/Time.h>
+
+#include "tf/transform_broadcaster.h"
+
+#define USAGE "stageros <worldfile>"
+#define ODOM "odom"
+#define BASE_SCAN "base_scan"
+#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
+#define CMD_VEL "cmd_vel"
+
+// Our node
+class StageNode : public ros::Node
+{
+ private:
+ // Messages that we'll send or receive
+ robot_msgs::PoseDot *velMsgs;
+ laser_scan::LaserScan *laserMsgs;
+ deprecated_msgs::RobotBase2DOdom *odomMsgs;
+ robot_msgs::PoseWithRatesStamped *groundTruthMsgs;
+ roslib::Time timeMsg;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ boost::mutex msg_lock;
+
+ // The models that we're interested in
+ std::vector<Stg::StgModelLaser *> lasermodels;
+ std::vector<Stg::StgModelPosition *> positionmodels;
+
+ // A helper function that is executed for each stage model. We use it
+ // to search for models of interest.
+ static void ghfunc(gpointer key, Stg::StgModel* mod, StageNode* node);
+
+ // Appends the given robot ID to the given message name. If omitRobotID
+ // is true, an unaltered copy of the name is returned.
+ const char *mapName(const char *name, size_t robotID);
+
+ tf::TransformBroadcaster tf;
+
+ // Last time that we received a velocity command
+ ros::Time base_last_cmd;
+ ros::Duration base_watchdog_timeout;
+
+ // Current simulation time
+ ros::Time sim_time;
+
+ public:
+ // Constructor; stage itself needs argc/argv. fname is the .world file
+ // that stage should load.
+ StageNode(int argc, char** argv, bool gui, const char* fname);
+ ~StageNode();
+
+ // Subscribe to models of interest. Currently, we find and subscribe
+ // to the first 'laser' model and the first 'position' model. Returns
+ // 0 on success (both models subscribed), -1 otherwise.
+ int SubscribeModels();
+
+ // Do one update of the simulator. May pause if the next update time
+ // has not yet arrived.
+ void Update();
+
+ // Message callback for a MsgBaseVel message, which set velocities.
+ void cmdvelReceived();
+
+ // The main simulator object
+ Stg::StgWorld* world;
+};
+
+// since stageros is single-threaded, this is OK. revisit if that changes!
+const char *
+StageNode::mapName(const char *name, size_t robotID)
+{
+ if (positionmodels.size() > 1)
+ {
+ static char buf[100];
+ snprintf(buf, sizeof(buf), "/robot_%d/%s", robotID, name);
+ return buf;
+ }
+ else
+ return name;
+}
+
+void
+StageNode::ghfunc(gpointer key,
+ Stg::StgModel* mod,
+ StageNode* node)
+{
+ if (dynamic_cast<Stg::StgModelLaser *>(mod))
+ node->lasermodels.push_back(dynamic_cast<Stg::StgModelLaser *>(mod));
+ if (dynamic_cast<Stg::StgModelPosition *>(mod))
+ node->positionmodels.push_back(dynamic_cast<Stg::StgModelPosition *>(mod));
+}
+
+void
+StageNode::cmdvelReceived()
+{
+ boost::mutex::scoped_lock lock(msg_lock);
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ {
+ this->positionmodels[r]->SetSpeed(this->velMsgs[r].vel.vx,
+ this->velMsgs[r].vel.vy,
+ this->velMsgs[r].ang_vel.vz);
+ }
+ this->base_last_cmd = this->sim_time;
+}
+
+StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) :
+ ros::Node("stageros"),
+ tf(*this)
+{
+ this->sim_time.fromSec(0.0);
+ this->base_last_cmd.fromSec(0.0);
+ double t;
+ param("~base_watchdog_timeout", t, 0.2);
+ this->base_watchdog_timeout.fromSec(t);
+
+ // initialize libstage
+ Stg::Init( &argc, &argv );
+
+ if(gui)
+ this->world = new Stg::StgWorldGui(800, 700, "Stage (ROS)");
+ else
+ this->world = new Stg::StgWorld();
+
+ this->world->Load(fname);
+
+ this->world->ForEachModel((GHFunc)ghfunc, this);
+ if (lasermodels.size() != positionmodels.size())
+ {
+ ROS_FATAL("number of position models and laser models must be equal in "
+ "the world file.");
+ ROS_BREAK();
+ }
+ size_t numRobots = positionmodels.size();
+ ROS_INFO("found %d position model(s) in the file", numRobots);
+
+ this->velMsgs = new robot_msgs::PoseDot[numRobots];
+ this->laserMsgs = new laser_scan::LaserScan[numRobots];
+ this->odomMsgs = new deprecated_msgs::RobotBase2DOdom[numRobots];
+ this->groundTruthMsgs = new robot_msgs::PoseWithRatesStamped[numRobots];
+}
+
+
+// Subscribe to models of interest. Currently, we find and subscribe
+// to the first 'laser' model and the first 'position' model. Returns
+// 0 on success (both models subscribed), -1 otherwise.
+//
+// Eventually, we should provide a general way to map stage models onto ROS
+// topics, similar to Player .cfg files.
+int
+StageNode::SubscribeModels()
+{
+ setParam("/use_sim_time", true);
+
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ {
+ if(this->lasermodels[r])
+ this->lasermodels[r]->Subscribe();
+ else
+ {
+ puts("no laser");
+ return(-1);
+ }
+ if(this->positionmodels[r])
+ this->positionmodels[r]->Subscribe();
+ else
+ {
+ puts("no position");
+ return(-1);
+ }
+ advertise<laser_scan::LaserScan>(mapName(BASE_SCAN,r), 10);
+ advertise<deprecated_msgs::RobotBase2DOdom>(mapName(ODOM,r), 10);
+ advertise<robot_msgs::PoseWithRatesStamped>(
+ mapName(BASE_POSE_GROUND_TRUTH,r), 10);
+ subscribe(mapName(CMD_VEL,r), velMsgs[r], &StageNode::cmdvelReceived, 10);
+ }
+ advertise<roslib::Time>("time",10);
+ return(0);
+}
+
+StageNode::~StageNode()
+{
+ delete[] velMsgs;
+ delete[] laserMsgs;
+ delete[] odomMsgs;
+ delete[] groundTruthMsgs;
+}
+
+void
+StageNode::Update()
+{
+ // Wait until it's time to update
+ this->world->PauseUntilNextUpdateTime();
+ boost::mutex::scoped_lock lock(msg_lock);
+
+ // Let the simulator update (it will sleep if there's time)
+ this->world->Update();
+
+ this->sim_time.fromSec(world->SimTimeNow() / 1e6);
+
+ // TODO make this only affect one robot if necessary
+ if((this->base_watchdog_timeout.toSec() > 0.0) &&
+ ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
+ {
+ for (size_t r = 0; r < this->positionmodels.size(); r++)
+ this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
+ }
+
+ // Get latest laser data
+ for (size_t r = 0; r < this->lasermodels.size(); r++)
+ {
+ Stg::stg_laser_sample_t* samples = this->lasermodels[r]->GetSamples();
+ if(samples)
+ {
+ // Translate into ROS message format and publish
+ Stg::stg_laser_cfg_t cfg = this->lasermodels[r]->GetConfig();
+ this->laserMsgs[r].angle_min = -cfg.fov/2.0;
+ this->laserMsgs[r].angle_max = +cfg.fov/2.0;
+ this->laserMsgs[r].angle_increment = cfg.fov/(double)(cfg.sample_count-1);
+ this->laserMsgs[r].range_min = 0.0;
+ this->laserMsgs[r].range_max = cfg.range_bounds.max;
+ this->laserMsgs[r].ranges.resize(cfg.sample_count);
+ this->laserMsgs[r].intensities.resize(cfg.sample_count);
+ for(unsigned int i=0;i<cfg.sample_count;i++)
+ {
+ this->laserMsgs[r].ranges[i] = samples[i].range;
+ this->laserMsgs[r].intensities[i] = (uint8_t)samples[i].reflectance;
+ }
+
+ this->laserMsgs[r].header.frame_id = mapName("base_laser", r);
+ this->laserMsgs[r].header.stamp = sim_time;
+ publish(mapName(BASE_SCAN,r), this->laserMsgs[r]);
+ }
+
+ // Also publish the base->base_laser Tx. This could eventually move
+ // into being retrieved from the param server as a static Tx.
+ Stg::stg_pose_t lp = this->lasermodels[r]->GetPose();
+ tf.sendTransform(tf::Stamped<tf::Transform>
+ (tf::Transform(tf::Quaternion(lp.a, 0, 0),
+ tf::Point(lp.x, lp.y, 0.15)),
+ sim_time, mapName("base_laser", r), mapName("base_link", r)));
+ // Send the identity transform between base_footprint and base_link
+ tf::Transform txIdentity(tf::Quaternion(0, 0, 0), tf::Point(0, 0, 0));
+ tf.sendTransform(tf::Stamped<tf::Transform>
+ (txIdentity,
+ sim_time, mapName("base_link", r), mapName("base_footprint", r)));
+ // Get latest odometry data
+ // Translate into ROS message format and publish
+ this->odomMsgs[r].pos.x = this->positionmodels[r]->est_pose.x;
+ this->odomMsgs[r].pos.y = this->positionmodels[r]->est_pose.y;
+ this->odomMsgs[r].pos.th = this->positionmodels[r]->est_pose.a;
+ Stg::stg_velocity_t v = this->positionmodels[r]->GetVelocity();
+ this->odomMsgs[r].vel.x = v.x;
+ this->odomMsgs[r].vel.y = v.y;
+ this->odomMsgs[r].vel.th = v.a;
+ this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
+ this->odomMsgs[r].header.frame_id = mapName("odom", r);
+ this->odomMsgs[r].header.stamp = sim_time;
+
+ publish(mapName(ODOM,r),this->odomMsgs[r]);
+
+ tf::Stamped<tf::Transform> tx(
+ tf::Transform(
+ tf::Quaternion(odomMsgs[r].pos.th, 0, 0),
+ tf::Point(odomMsgs[r].pos.x, odomMsgs[r].pos.y, 0.0)),
+ sim_time, mapName("base_footprint", r), mapName("odom", r));
+ this->tf.sendTransform(tx);
+
+ // Also publish the ground truth pose
+ Stg::stg_pose_t gpose = this->positionmodels[r]->GetGlobalPose();
+ // Note that we correct for Stage's screwed-up coord system.
+ tf::Transform gt(tf::Quaternion(gpose.a-M_PI/2.0, 0, 0),
+ tf::Point(gpose.y, -gpose.x, 0.0));
+
+ this->groundTruthMsgs[r].pos.position.x = gt.getOrigin().x();
+ this->groundTruthMsgs[r].pos.position.y = gt.getOrigin().y();
+ this->groundTruthMsgs[r].pos.position.z = gt.getOrigin().z();
+ this->groundTruthMsgs[r].pos.orientation.x = gt.getRotation().x();
+ this->groundTruthMsgs[r].pos.orientation.y = gt.getRotation().y();
+ this->groundTruthMsgs[r].pos.orientation.z = gt.getRotation().z();
+ this->groundTruthMsgs[r].pos.orientation.w = gt.getRotation().w();
+
+ this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
+ this->groundTruthMsgs[r].header.stamp = sim_time;
+
+ publish(mapName(BASE_POSE_GROUND_TRUTH,r), this->groundTruthMsgs[r]);
+
+ }
+
+ this->timeMsg.rostime = sim_time;
+ publish("time", this->timeMsg);
+}
+
+int
+main(int argc, char** argv)
+{
+ if( argc < 2 )
+ {
+ puts(USAGE);
+ exit(-1);
+ }
+
+ ros::init(argc,argv);
+
+ bool gui = true;
+ for(int i=0;i<(argc-1);i++)
+ {
+ if(!strcmp(argv[i], "-g"))
+ gui = false;
+ }
+
+ StageNode sn(argc-1,argv,gui,argv[argc-1]);
+
+ if(sn.SubscribeModels() != 0)
+ exit(-1);
+
+ while(sn.ok() && !sn.world->TestQuit())
+ {
+ sn.Update();
+ }
+
+ exit(0);
+}
+
Property changes on: pkg/trunk/3rdparty/stage/src/stageros.cpp
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/3rdparty/stage/test
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/3rdparty/stage/world
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" >
<remap from="scan" to="base_scan" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" >
<remap from="scan" to="base_scan" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
<param name="robot_0/wavefront_player_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
<node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" output="screen"/>
<param name="robot_radius" value="0.325"/>
<!-- node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="6"/>
<param name="pf_min_samples" value="100"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="6"/>
<param name="pf_min_samples" value="100"/>
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-03-27 01:49:03 UTC (rev 13027)
@@ -8,7 +8,7 @@
<depend package="nav_view_sdl"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="rosstage"/>
+ <depend package="stage"/>
<depend package="map_server"/>
<depend package="wavefront_player"/>
<depend package="highlevel_controllers"/>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/controllers_sim.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -2,5 +2,6 @@
<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>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-03-27 00:22:13 UTC (rev 13026)
+++ pkg/trunk/demos/tabletop_manipulation/launch/gazebo.launch 2009-03-27 01:49:03 UTC (rev 13027)
@@ -1,22 +1,20 @@
<launch>
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/pr2_defs/robots for full pr2 -->
- <group name="wg">
- <!-- send pr2.xml to param server -->
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+ <!-- 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" 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)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
+ <!-- start gazebo -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find tabletop_manipulation)/gazebo/simple.world" respawn="false">
+ <env name="LD...
[truncated message content] |
|
From: <ge...@us...> - 2009-03-27 02:21:08
|
Revision: 13029
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13029&view=rev
Author: gerkey
Date: 2009-03-27 02:20:59 +0000 (Fri, 27 Mar 2009)
Log Message:
-----------
Moved to new rosbuild macro
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2009-03-27 02:18:33 UTC (rev 13028)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2009-03-27 02:20:59 UTC (rev 13029)
@@ -21,17 +21,4 @@
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
# Download needed data file
-# This code is wrapped up in a new macro, which hasn't yet been merged to
-# the stable branch of ROS.
-#rospack_download_test_data(http://pr.willowgarage.com/data/calonder_descriptor/current.rtc current.rtc)
-set(_url http://pr.willowgarage.com/data/point_cloud_assembler/test_assembler.bag)
-set(_filename test/test_assembler.bag)
-find_package(Wget REQUIRED)
-add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/${_filename}
- COMMAND ${WGET_EXECUTABLE} -r ${_url} -O ${PROJECT_SOURCE_DIR}/${_filename}
- VERBATIM)
-# Create a legal target name, in case the target name has slashes in it
-string(REPLACE "/" "_" _testname download_data_${_filename})
-add_custom_target(${_testname}
- DEPENDS ${PROJECT_SOURCE_DIR}/${_filename})
-add_dependencies(tests ${_testname})
+rospack_download_test_data(http://pr.willowgarage.com/data/point_cloud_assembler/test_assembler.bag test/test_assembler.bag)
Modified: pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt 2009-03-27 02:18:33 UTC (rev 13028)
+++ pkg/trunk/estimation/robot_pose_ekf/CMakeLists.txt 2009-03-27 02:20:59 UTC (rev 13029)
@@ -20,17 +20,4 @@
rospack_add_rostest(${CMAKE_CURRENT_SOURCE_DIR}/test/test_robot_pose_ekf.launch)
# Download needed data file
-# This code is wrapped up in a new macro, which hasn't yet been merged to
-# the stable branch of ROS.
-#rospack_download_test_data(http://pr.willowgarage.com/data/calonder_descriptor/current.rtc current.rtc)
-set(_url http://pr.willowgarage.com/data/robot_pose_ekf/ekf_test.bag)
-set(_filename test/ekf_test.bag)
-find_package(Wget REQUIRED)
-add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/${_filename}
- COMMAND ${WGET_EXECUTABLE} -r ${_url} -O ${PROJECT_SOURCE_DIR}/${_filename}
- VERBATIM)
-# Create a legal target name, in case the target name has slashes in it
-string(REPLACE "/" "_" _testname download_data_${_filename})
-add_custom_target(${_testname}
- DEPENDS ${PROJECT_SOURCE_DIR}/${_filename})
-add_dependencies(tests ${_testname})
+rospack_download_test_data(http://pr.willowgarage.com/data/robot_pose_ekf/ekf_test.bag test/ekf_test.bag)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-03-27 22:47:01
|
Revision: 13064
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13064&view=rev
Author: stuglaser
Date: 2009-03-27 22:46:54 +0000 (Fri, 27 Mar 2009)
Log Message:
-----------
More plugging in
Modified Paths:
--------------
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/demos/plug_in/outlet_pose_filter.py
pkg/trunk/demos/plug_in/plug_in2.py
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
Modified: pkg/trunk/demos/plug_in/move_to_pickup.py
===================================================================
--- pkg/trunk/demos/plug_in/move_to_pickup.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/move_to_pickup.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -210,7 +210,8 @@
pickup()
pub.publish(Float64(0.6))
- rospy.spin()
+ sleep(2)
+ #rospy.spin()
finally:
for name in controllers:
Modified: pkg/trunk/demos/plug_in/outlet_pose_filter.py
===================================================================
--- pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -79,6 +79,7 @@
tf_msg.transforms[0].header.stamp = rospy.rostime.get_rostime()
tf_pub.publish(tf_msg)
seq += 1
+ outlet_msg = track_outlet_pose.msg
else:
outlet_msg = track_outlet_pose.msg
Modified: pkg/trunk/demos/plug_in/plug_in2.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in2.py 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/plug_in2.py 2009-03-27 22:46:54 UTC (rev 13064)
@@ -138,6 +138,8 @@
else:
# trajectory controller is already spawned
+ # TODO: actually check if the traj controller is up. Spawn if not
+
p_up = PoseStamped()
p_up.header.frame_id = 'base_link'
p_up.header.stamp = rospy.get_rostime()
@@ -150,19 +152,26 @@
p_face.pose.position = xyz(0.33, -0.09, 0.37)
p_face.pose.orientation = Quaternion(-0.04, 0.26, -0.00, 0.96)
- p_stage = PoseStamped()
- p_stage.header.frame_id = 'outlet_pose'
- p_stage.header.stamp = rospy.get_rostime()
- p_stage.pose.position = xyz(-0.12, 0.0, 0.0)
- p_stage.pose.orientation = rpy(0,0.3,0)
+ p_stage1 = PoseStamped()
+ p_stage1.header.frame_id = 'outlet_pose'
+ p_stage1.header.stamp = rospy.get_rostime()
+ p_stage1.pose.position = xyz(-0.12, 0.0, 0.04)
+ p_stage1.pose.orientation = rpy(0,0.3,0)
+ p_stage2 = PoseStamped()
+ p_stage2.header.frame_id = 'outlet_pose'
+ p_stage2.header.stamp = rospy.get_rostime()
+ p_stage2.pose.position = xyz(-0.07, 0.0, 0.04)
+ p_stage2.pose.orientation = rpy(0,0.3,0)
+
try:
print "Waiting for the trajectory controller"
move_arm = rospy.ServiceProxy('cartesian_trajectory_right/move_to', MoveToPose)
print "Staging the plug"
- move_arm(p_up)
- move_arm(p_face)
- move_arm(p_stage)
+ p_up.header.stamp = rospy.get_rostime(); move_arm(p_up)
+ p_face.header.stamp = rospy.get_rostime(); move_arm(p_face)
+ p_stage1.header.stamp = rospy.get_rostime(); move_arm(p_stage1)
+ p_stage2.header.stamp = rospy.get_rostime(); move_arm(p_stage2)
except rospy.ServiceException, e:
print "move_to service failed: %s" % e
@@ -252,7 +261,8 @@
rospy.wait_for_service("/arm_hybrid/set_tool_frame")
if rospy.is_shutdown(): return
set_tool_frame = rospy.ServiceProxy("/arm_hybrid/set_tool_frame", SetPoseStamped)
- set_tool_frame(plug_pose)
+ #set_tool_frame(plug_pose)
+ set_tool_frame(track_plug_pose.msg)
print "Tool frame set"
time.sleep(1)
Modified: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-27 22:46:54 UTC (rev 13064)
@@ -37,13 +37,15 @@
#include "robot_msgs/TaskFrameFormalism.h"
const double MIN_STANDOFF = 0.035;
+const double SUCCESS_THRESHOLD = 0.025;
-enum {MEASURING, MOVING, PUSHING, FORCING};
+enum {MEASURING, MOVING, PUSHING, FORCING, HOLDING};
int g_state = MEASURING;
ros::Time g_started_pushing = ros::Time::now(),
g_started_forcing = ros::Time::now(),
- g_stopped_forcing = ros::Time::now();
+ g_stopped_forcing = ros::Time::now(),
+ g_started_holding = ros::Time::now();
boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > g_mn;
boost::scoped_ptr<tf::TransformListener> TF;
@@ -135,7 +137,7 @@
printf("Offset: (% 0.3lf, % 0.3lf, % 0.3lf)\n", offset.x(), offset.y(), offset.z());
if (g_started_pushing + ros::Duration(5.0) < ros::Time::now())
{
- if (false) // if (in_outlet)
+ if (offset.x() > SUCCESS_THRESHOLD) // if (in_outlet)
{
g_state = FORCING;
}
@@ -148,10 +150,16 @@
}
case FORCING: {
- g_state = MEASURING;
+ if (ros::Time::now() > g_started_forcing + ros::Duration(1.0))
+ g_state = HOLDING;
break;
}
+
+ case HOLDING: {
+ //g_state = MEASURING;
+ break;
}
+ }
// Handles the transitions
@@ -234,7 +242,7 @@
tff.mode.rot.x = 2;
tff.mode.rot.y = 2;
tff.mode.rot.z = 2;
- tff.value.vel.x = 40;
+ tff.value.vel.x = 50;
tff.value.vel.y = mech_offset.getOrigin().y();
tff.value.vel.z = mech_offset.getOrigin().z();
tff.value.rot.x = 0.0;
@@ -249,7 +257,27 @@
ros::Node::instance()->publish("/arm_hybrid/command", tff);
break;
}
+
+ case HOLDING: {
+ printf("enter HOLDING\n");
+ g_started_holding = ros::Time::now();
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 1;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = 4;
+ tff.value.vel.y = mech_offset.getOrigin().y();
+ tff.value.vel.z = mech_offset.getOrigin().z();
+ mech_offset.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ break;
}
+ }
}
last_standoff = standoff;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-27 22:27:12 UTC (rev 13063)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml 2009-03-27 22:46:54 UTC (rev 13064)
@@ -299,7 +299,7 @@
<axis xyz="0 1 0" />
<limit min="-0.55" max="1.047" effort="12.21"
velocity="100" k_velocity="1.5"
- safety_length_min="0.02" safety_length_max="0.1" k_position="100" />
+ safety_length_min="0.02" safety_length_max="0.02" k_position="100" />
<calibration reference_position="${-0.195+cal_head_tilt_flag}" values="0 0" />
<joint_properties damping="1.0" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ve...@us...> - 2009-03-28 00:04:27
|
Revision: 13070
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13070&view=rev
Author: veedee
Date: 2009-03-28 00:04:21 +0000 (Sat, 28 Mar 2009)
Log Message:
-----------
FLANN skel in, changed ANN distances to floats from doubles
Modified Paths:
--------------
pkg/trunk/3rdparty/ANN/Makefile
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp
pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp
pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp
pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
Added Paths:
-----------
pkg/trunk/3rdparty/ANN/distance.patch
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp
Modified: pkg/trunk/3rdparty/ANN/Makefile
===================================================================
--- pkg/trunk/3rdparty/ANN/Makefile 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/3rdparty/ANN/Makefile 2009-03-28 00:04:21 UTC (rev 13070)
@@ -4,7 +4,7 @@
TARBALL_URL = http://www.cs.umd.edu/~mount/ANN/Files/1.1.1/ann_1.1.1.tar.gz
UNPACK_CMD = tar xfz
SOURCE_DIR = build/ann_1.1.1
-TARBALL_PATCH = gcc43_shared.patch fPIC.patch
+TARBALL_PATCH = gcc43_shared.patch fPIC.patch distance.patch
include $(shell rospack find mk)/download_unpack_build.mk
Added: pkg/trunk/3rdparty/ANN/distance.patch
===================================================================
--- pkg/trunk/3rdparty/ANN/distance.patch (rev 0)
+++ pkg/trunk/3rdparty/ANN/distance.patch 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,11 @@
+--- include/ANN/ANN.h.orig 2009-03-28 00:51:04.000000000 +0100
++++ include/ANN/ANN.h 2009-03-28 00:51:24.000000000 +0100
+@@ -152,7 +152,7 @@
+ //----------------------------------------------------------------------
+
+ typedef float ANNcoord; // coordinate data type
+-typedef double ANNdist; // distance data type
++typedef float ANNdist; // distance data type
+
+ //----------------------------------------------------------------------
+ // ANNidx
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -477,7 +477,7 @@
processed.resize (nr_points, false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (int i = 0; i < nr_points; i++)
{
@@ -656,7 +656,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (points_down.pts.size ());
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (int i = 0; i < (int)points_down.pts.size (); i++)
@@ -724,7 +724,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (point_indices.size ());
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (unsigned int i = 0; i < point_indices.size (); i++)
@@ -793,7 +793,7 @@
// Allocate enough space for point indices
vector<vector<int> > points_k_indices (nr_points);
- vector<double> distances;
+ vector<float> distances;
// Get the nearest neighbors for all the point indices in the bounds
for (int i = 0; i < nr_points; i++)
@@ -953,10 +953,10 @@
cloud_kdtree::KdTree *kdtree = new cloud_kdtree::KdTreeANN (points, indices);
// Get the nearest neighbors for all the point indices in the bounds
+ vector<float> distances;
for (unsigned int i = 0; i < cluster.size (); i++)
{
vector<int> points_k_indices;
- vector<double> distances;
kdtree->radiusSearch (points.pts[cluster.at (i)], dist_thresh, points_k_indices, distances);
// Copy the inliers
Modified: pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp
===================================================================
--- pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/plug_onbase_detector/src/plug_onbase_detector.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -318,7 +318,7 @@
processed.resize (indices.size (), false);
vector<int> nn_indices;
- vector<double> nn_distances;
+ vector<float> nn_distances;
// Process all points in the indices vector
for (unsigned int i = 0; i < indices.size (); i++)
{
Modified: pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/CMakeLists.txt 2009-03-28 00:04:21 UTC (rev 13070)
@@ -48,13 +48,9 @@
target_link_libraries (bin/test_cloud_io cloud_io)
# ---[ Cloud Kd-Tree library
-# rospack_add_library (cloud_kdtree
-# src/cloud_kdtree/convert.cpp
-# src/cloud_kdtree/search.cpp
-# )
-
rospack_add_library (cloud_kdtree
src/cloud_kdtree/kdtree_ann.cpp
+ src/cloud_kdtree/kdtree_flann.cpp
)
rospack_add_gtest (bin/test_cloud_kdtree test/cloud_kdtree/test_kdtree.cpp)
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -71,15 +71,15 @@
/** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */
virtual ~KdTree () { }
- virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
- virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
- virtual inline void nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances) = 0;
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
+ virtual inline void nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) = 0;
- virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
- virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
- virtual inline bool radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ virtual inline bool radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX) = 0;
Modified: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_ann.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -115,8 +115,8 @@
m_lock_.unlock ();
}
- virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances);
- virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Search for k-nearest neighbors for the given query point.
@@ -126,7 +126,7 @@
* \param k_distances the resultant point distances
*/
virtual inline void
- nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
if (index >= nr_points_)
return;
@@ -137,8 +137,8 @@
return;
}
- virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances, int max_nn = INT_MAX);
- virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Search for all the nearest neighbors of the query point in a given radius.
@@ -149,7 +149,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
virtual inline bool
- radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn = INT_MAX)
{
radius *= radius;
Added: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h (rev 0)
+++ pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * $Id$
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_KDTREE_KDTREE_FLANN_H_
+#define _CLOUD_KDTREE_KDTREE_FLANN_H_
+
+#include "point_cloud_mapping/kdtree/kdtree.h"
+
+#include <boost/thread/mutex.hpp>
+
+#include <flann.h>
+
+namespace cloud_kdtree
+{
+ class KdTreeFLANN : public KdTree
+ {
+ public:
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Constructor for KdTree.
+ * \param points the ROS point cloud data array
+ */
+ KdTreeFLANN (const robot_msgs::PointCloud &points)
+ {
+ epsilon_ = 0.0; // default error bound value
+ dim_ = 3; // default number of dimensions (3 = xyz)
+
+ // Allocate enough data
+ nr_points_ = convertCloudToArray (points);
+ if (nr_points_ == 0)
+ {
+ ROS_ERROR ("[KdTreeFLANN] Could not create kD-tree for %d points!", nr_points_);
+ return;
+ }
+
+ // Create the kd_tree representation
+ float speedup;
+ FLANNParameters flann_param;
+ flann_param.algorithm = KDTREE;
+ flann_param.log_level = LOG_NONE;
+ flann_param.log_destination = NULL;
+
+ flann_param.checks = 32;
+ flann_param.trees = 8;
+ flann_param.branching = 32;
+ flann_param.iterations = 7;
+ flann_param.target_precision = -1;
+
+ m_lock_.lock ();
+ index_id_ = flann_build_index (points_, nr_points_, dim_, &speedup, &flann_param);
+ m_lock_.unlock ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Constructor for KdTree.
+ * \note ATTENTION: This method breaks the 1-1 mapping between the indices returned using \a getNeighborsIndices
+ * and the ones from the \a points message ! When using this method, make sure to get the underlying point data
+ * using the \a getPoint method
+ * \param points the ROS point cloud data array
+ * \param indices the point cloud indices
+ */
+ KdTreeFLANN (const robot_msgs::PointCloud &points, const std::vector<int> &indices)
+ {
+ epsilon_ = 0.0; // default error bound value
+ dim_ = 3; // default number of dimensions (3 = xyz)
+
+ // Allocate enough data
+ nr_points_ = convertCloudToArray (points, indices);
+ if (nr_points_ == 0)
+ {
+ ROS_ERROR ("[KdTreeFLANN] Could not create kD-tree for %d points!", nr_points_);
+ return;
+ }
+
+ // Create the kd_tree representation
+ float speedup;
+ m_lock_.lock ();
+ flann_param_.algorithm = KDTREE;
+ flann_param_.log_level = LOG_NONE;
+ flann_param_.log_destination = NULL;
+
+ flann_param_.checks = 32;
+ flann_param_.trees = 8;
+ flann_param_.branching = 32;
+ flann_param_.iterations = 7;
+ flann_param_.target_precision = -1;
+
+ index_id_ = flann_build_index (points_, nr_points_, dim_, &speedup, &flann_param_);
+ m_lock_.unlock ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */
+ virtual ~KdTreeFLANN ()
+ {
+ m_lock_.lock ();
+
+ // Data array cleanup
+ if (points_ != NULL && nr_points_ != 0)
+ free (points_);
+
+ // ANN Cleanup
+ flann_free_index (index_id_, &flann_param_);
+
+ m_lock_.unlock ();
+ }
+
+ virtual void nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+ virtual void nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param index the index in \a points representing the query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ virtual inline void
+ nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ if (index >= nr_points_)
+ return;
+
+ m_lock_.lock ();
+ flann_find_nearest_neighbors_index (index_id_, &points_[index], 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+ return;
+ }
+
+ virtual bool radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+ virtual bool radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances, int max_nn = INT_MAX);
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param index the index in \a points representing the query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ virtual inline bool
+ radiusSearch (int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn = INT_MAX)
+ {
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+ //int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (points_[index], radius, 0, NULL, NULL, epsilon_);
+ m_lock_.unlock ();
+
+ // No neighbors found ? Return false
+ if (neighbors_in_radius_ == 0)
+ return (false);
+
+ if (neighbors_in_radius_ > max_nn) neighbors_in_radius_ = max_nn;
+ k_indices.resize (neighbors_in_radius_);
+ k_distances.resize (neighbors_in_radius_);
+
+ m_lock_.lock ();
+ flann_radius_search (index_id_, &points_[index], &k_indices[0], &k_distances[0], nr_points_, radius, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ return (true);
+ }
+
+ private:
+
+ int convertCloudToArray (const robot_msgs::PointCloud &ros_cloud);
+ int convertCloudToArray (const robot_msgs::PointCloud &ros_cloud, const std::vector<int> &indices);
+
+ private:
+
+ boost::mutex m_lock_;
+
+ /** \brief A FL-ANN type index reference */
+ FLANN_INDEX index_id_;
+
+ /** \brief A pointer to a FL-ANN parameter structure */
+ FLANNParameters flann_param_;
+
+ /** \brief Internal pointer to data */
+ float* points_;
+
+ /** \brief Number of points in the tree */
+ int nr_points_;
+ /** \brief Tree dimensionality (i.e. the number of dimensions per point) */
+ int dim_;
+ };
+
+}
+
+#endif
Property changes on: pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/kdtree/kdtree_flann.h
___________________________________________________________________
Added: svn:keywords
+ Id
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp 2009-03-27 23:53:28 UTC (rev 13069)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_ann.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -42,7 +42,7 @@
* \param k_distances the resultant point distances
*/
void
- KdTreeANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ KdTreeANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
k_indices.resize (k);
k_distances.resize (k);
@@ -67,7 +67,7 @@
* \param k_distances the resultant point distances
*/
void
- KdTreeANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<double> &k_distances)
+ KdTreeANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
{
if (index >= (int)points.pts.size ())
return;
@@ -95,7 +95,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
bool
- KdTreeANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ KdTreeANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn)
{
ANNpoint p = annAllocPt (3);
@@ -135,7 +135,7 @@
* \param max_nn if given, bounds the maximum returned neighbors to this value
*/
bool
- KdTreeANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<double> &k_distances,
+ KdTreeANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
int max_nn)
{
ANNpoint p = annAllocPt (3);
Added: pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp (rev 0)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/kdtree_flann.cpp 2009-03-28 00:04:21 UTC (rev 13070)
@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * $Id$
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#include "point_cloud_mapping/kdtree/kdtree_flann.h"
+
+namespace cloud_kdtree
+{
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param p_q the given query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ void
+ KdTreeFLANN::nearestKSearch (const robot_msgs::Point32 &p_q, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ k_indices.resize (k);
+ k_distances.resize (k);
+
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
+
+ m_lock_.lock ();
+// int* nn_idx_ = (int*) malloc(tcount*nn*sizeof(int));
+// float* nn_dists_ = (float*) malloc(tcount*nn*sizeof(float));
+ flann_find_nearest_neighbors_index (index_id_, p, 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for k-nearest neighbors for the given query point.
+ * \param points the point cloud data
+ * \param index the index in \a points representing the query point
+ * \param k the number of neighbors to search for
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ */
+ void
+ KdTreeFLANN::nearestKSearch (const robot_msgs::PointCloud &points, int index, int k, std::vector<int> &k_indices, std::vector<float> &k_distances)
+ {
+ if (index >= (int)points.pts.size ())
+ return;
+
+ k_indices.resize (k);
+ k_distances.resize (k);
+
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = points.pts.at (index).x; p[1] = points.pts.at (index).y; p[2] = points.pts.at (index).z;
+
+ m_lock_.lock ();
+ flann_find_nearest_neighbors_index (index_id_, p, 1, &k_indices[0], &k_distances[0], k, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return;
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param p_q the given query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ bool
+ KdTreeFLANN::radiusSearch (const robot_msgs::Point32 &p_q, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn)
+ {
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = p_q.x; p[1] = p_q.y; p[2] = p_q.z;
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+// int neighbors_in_radius_ = ann_kd_tree_->annkFRSearch (p, radius, 0, NULL, NULL, epsilon_);
+ m_lock_.unlock ();
+
+ // No neighbors found ? Return false
+ if (neighbors_in_radius_ == 0)
+ {
+ free (p);
+ return (false);
+ }
+
+ if (neighbors_in_radius_ > max_nn) neighbors_in_radius_ = max_nn;
+ k_indices.resize (neighbors_in_radius_);
+ k_distances.resize (neighbors_in_radius_);
+
+ m_lock_.lock ();
+ flann_radius_search (index_id_, p, &k_indices[0], &k_distances[0], nr_points_, radius, flann_param_.checks, &flann_param_);
+ m_lock_.unlock ();
+
+ free (p);
+ return (true);
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Search for all the nearest neighbors of the query point in a given radius.
+ * \param points the point cloud data
+ * \param index the index in \a points representing the query point
+ * \param radius the radius of the sphere bounding all of p_q's neighbors
+ * \param k_indices the resultant point indices
+ * \param k_distances the resultant point distances
+ * \param max_nn if given, bounds the maximum returned neighbors to this value
+ */
+ bool
+ KdTreeFLANN::radiusSearch (const robot_msgs::PointCloud &points, int index, double radius, std::vector<int> &k_indices, std::vector<float> &k_distances,
+ int max_nn)
+ {
+ float* p = (float*)malloc (3 * sizeof (float));
+ p[0] = points.pts.at (index).x; p[1] = points.pts.at (index).y; p[2] = points.pts.at (index).z;
+ radius *= radius;
+
+ int neighbors_in_radius_;
+ m_lock_.lock ();
+// int neighbors_in_radius_ = ann_kd_tree_->annkFRSear...
[truncated message content] |
|
From: <tf...@us...> - 2009-03-28 00:50:16
|
Revision: 13075
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13075&view=rev
Author: tfoote
Date: 2009-03-28 00:50:06 +0000 (Sat, 28 Mar 2009)
Log Message:
-----------
proposal cleared for packages with wikipages and grandfathered
Modified Paths:
--------------
pkg/trunk/util/trajectory/manifest.xml
pkg/trunk/visualization/rviz/manifest.xml
Modified: pkg/trunk/util/trajectory/manifest.xml
===================================================================
--- pkg/trunk/util/trajectory/manifest.xml 2009-03-28 00:45:48 UTC (rev 13074)
+++ pkg/trunk/util/trajectory/manifest.xml 2009-03-28 00:50:06 UTC (rev 13075)
@@ -6,7 +6,7 @@
</description>
<author>Sachin Chitta</author>
<license>BSD</license>
-<review status="unreviewed" notes=""/>
+<review status="proposal cleared" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="rosconsole"/>
<depend package="angles"/>
Modified: pkg/trunk/visualization/rviz/manifest.xml
===================================================================
--- pkg/trunk/visualization/rviz/manifest.xml 2009-03-28 00:45:48 UTC (rev 13074)
+++ pkg/trunk/visualization/rviz/manifest.xml 2009-03-28 00:50:06 UTC (rev 13075)
@@ -6,7 +6,7 @@
</description>
<author>Josh Faust</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="proposal cleared" notes=""/>
<url>http://pr.willowgarage.com/wiki/rviz</url>
<depend package="ogre"/>
<depend package="ogre_tools"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-03-31 23:57:05
|
Revision: 13193
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13193&view=rev
Author: eitanme
Date: 2009-03-31 23:57:01 +0000 (Tue, 31 Mar 2009)
Log Message:
-----------
r19709@cib: eitan | 2009-03-20 16:31:43 -0700
Initial commit of new costmap... still a lot of work to be done
Added Paths:
-----------
pkg/trunk/world_models/new_costmap/
pkg/trunk/world_models/new_costmap/CMakeLists.txt
pkg/trunk/world_models/new_costmap/Makefile
pkg/trunk/world_models/new_costmap/include/
pkg/trunk/world_models/new_costmap/include/new_costmap/
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/manifest.xml
pkg/trunk/world_models/new_costmap/src/
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19709
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Added: pkg/trunk/world_models/new_costmap/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt (rev 0)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+set(ROS_BUILD_TYPE Release)
+rospack(new_costmap)
+genmsg()
+
+#rospack_add_boost_directories()
+
+rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
+#rospack_link_boost(costmap_2d thread)
Added: pkg/trunk/world_models/new_costmap/Makefile
===================================================================
--- pkg/trunk/world_models/new_costmap/Makefile (rev 0)
+++ pkg/trunk/world_models/new_costmap/Makefile 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,191 @@
+/*********************************************************************
+*
+* 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef COSTMAP_COSTMAP_2D_H_
+#define COSTMAP_COSTMAP_2D_H_
+
+#include <vector>
+#include <new_costmap/observation.h>
+
+namespace costmap_2d {
+ /**
+ * @class Costmap
+ * @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
+ */
+ class Costmap2D {
+ public:
+ /**
+ * @brief Constructor for a costmap
+ * @param meters_size_x The x size of the map in meters
+ * @param meters_size_y The y size of the map in meters
+ * @param resolution The resolution of the map in meters/cell
+ * @param origin_x The x origin of the map
+ * @param origin_y The y origin of the map
+ */
+ Costmap2D(double meters_size_x, double meters_size_y,
+ double resolution, double origin_x, double origin_y);
+
+ /**
+ * @brief Constructor for a costmap
+ * @param cells_size_x The x size of the map in cells
+ * @param cells_size_y The y size of the map in cells
+ * @param resolution The resolution of the map in meters/cell
+ * @param origin_x The x origin of the map
+ * @param origin_y The y origin of the map
+ */
+ Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
+ double resolution, double origin_x, double origin_y);
+
+ /**
+ * @brief Get the cost of a cell in the costmap
+ * @param mx The x coordinate of the cell
+ * @param my The y coordinate of the cell
+ * @return The cost of the cell
+ */
+ unsigned char getCellCost(unsigned int mx, unsigned int my) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates
+ * @param mx The x map coordinate
+ * @param my The y map coordinate
+ * @param wx Will be set to the associated world x coordinate
+ * @param wy Will be set to the associated world y coordinate
+ */
+ void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates
+ * @param wx The x world coordinate
+ * @param wy The y world coordinate
+ * @param mx Will be set to the associated map x coordinate
+ * @param my Will be set to the associated map y coordinate
+ * @return true if the conversion was successful (legal bounds) false otherwise
+ */
+ bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+
+ /**
+ * @brief Convert from map coordinates to world coordinates without checking for legal bounds
+ * @param wx The x world coordinate
+ * @param wy The y world coordinate
+ * @param mx Will be set to the associated map x coordinate
+ * @param my Will be set to the associated map y coordinate
+ */
+ void worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+
+ /**
+ * @brief Revert to the static map outside of a specified window centered at a world coordinate
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Insert new obstacles into the cost map
+ * @param obstacles The point clouds of obstacles to insert into the map
+ */
+ void updateObstacles(const std::vector<Observation>& obstacles);
+
+ /**
+ * @brief Clear freespace based on any number of planar scans
+ * @param clearing_scans The scans used to raytrace
+ */
+ void raytraceFreespace(const std::vector<Observation>& clearing_scans);
+
+ /**
+ * @brief Inflate obstacles within a given window centered at a point in world coordinates
+ * @param wx The x coordinate of the center point of the window in world space (meters)
+ * @param wy The y coordinate of the center point of the window in world space (meters)
+ * @param w_size_x The x size of the window in meters
+ * @param w_size_y The y size of the window in meters
+ */
+ void inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y);
+
+ /**
+ * @brief Accessor for the x size of the costmap in cells
+ * @return The x size of the costmap
+ */
+ unsigned int cellSizeX() const;
+
+ /**
+ * @brief Accessor for the y size of the costmap in cells
+ * @return The y size of the costmap
+ */
+ unsigned int cellSizeY() const;
+
+ /**
+ * @brief Accessor for the x size of the costmap in meters
+ * @return The x size of the costmap
+ */
+ double metersSizeX() const;
+
+ /**
+ * @brief Accessor for the y size of the costmap in meters
+ * @return The y size of the costmap
+ */
+ double metersSizeY() const;
+
+ /**
+ * @brief Accessor for the x origin of the costmap
+ * @return The x origin of the costmap
+ */
+ double originX() const;
+
+ /**
+ * @brief Accessor for the y origin of the costmap
+ * @return The y origin of the costmap
+ */
+ double originY() const;
+
+ /**
+ * @brief Accessor for the resolution of the costmap
+ * @return The resolution of the costmap
+ */
+ double resolution() const;
+
+ private:
+ unsigned int size_x_;
+ unsigned int size_y_;
+ double resolution_;
+ double origin_x_;
+ double origin_y_;
+ unsigned char* static_map_;
+ unsigned char* cost_map_;
+ };
+};
+
+#endif
Added: pkg/trunk/world_models/new_costmap/manifest.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/manifest.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/manifest.xml 2009-03-31 23:57:01 UTC (rev 13193)
@@ -0,0 +1,18 @@
+<package>
+<description>Cost map maintenance library</description>
+<author>Eitan Marder-Eppstein</author>
+<license>BSD</license>
+<review status="unreviewed" notes=""/>
+<depend package="rosconsole"/>
+<depend package="roscpp" />
+<depend package="std_msgs" />
+<depend package="robot_msgs" />
+<depend package="laser_scan" />
+<depend package="pr2_msgs" />
+<depend package="tf" />
+<depend package="robot_filter" />
+<depend package="robot_srvs" />
+<export>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnew_costmap"/>
+</export>
+</package>
Added: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp (rev 0)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:01 UTC (rev 13193)
@@ -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: Eitan Marder-Eppstein
+*********************************************************************/
+#include <new_costmap/costmap_2d.h>
+
+using namespace std;
+
+namespace costmap_2d{
+
+ Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
+ double resolution, double origin_x, double origin_y) : resolution_(resolution),
+ origin_x_(origin_x), origin_y_(origin_y) {
+ //make sure that we have a legal sized cost_map
+ ROS_ASSERT_MSG(meters_size_x > 0 && meters_size_y > 0, "Both the x and y dimensions of the costmap must be greater than 0.");
+
+ size_x_ = (unsigned int) (meters_size_x / resolution);
+ size_y_ = (unsigned int) (meters_size_y / resolution);
+ }
+
+ Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
+ double resolution, double origin_x, double origin_y) : size_x_(cells_size_x),
+ size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y) {}
+
+ unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
+ ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
+ return cost_map_[my * size_x_ + mx];
+ }
+
+ void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
+ wx = origin_x_ + (mx + 0.5) * resolution_;
+ wy = origin_y_ + (my + 0.5) * resolution_;
+ }
+
+ bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
+ if(wx < origin_x_ || wy < origin_y_)
+ return false;
+
+ mx = (int) ((wx - origin_x_) / resolution_);
+ my = (int) ((wy - origin_y_) / resolution_);
+
+ if(mx < size_x_ && my < size_y_)
+ return true;
+
+ return false;
+ }
+
+ void Costmap2D::worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const {
+ mx = (int) ((wx - origin_x_) / resolution_);
+ my = (int) ((wy - origin_y_) / resolution_);
+ }
+
+ void Costmap2D::resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y){
+ ROS_ASSERT_MSG(w_size_x >= 0 && w_size_y >= 0, "You cannot specify a negative size window");
+
+ double start_point_x = wx - w_size_x / 2;
+ double start_point_y = wy - w_size_y / 2;
+
+ unsigned int start_x, start_y, end_x, end_y;
+ //we'll do our own bounds checking for the window
+ worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
+ worldToMapNoBounds(start_point_x + w_size_x, start_point_y + w_size_y, end_x, end_y);
+
+ //check start bounds
+ start_x = start_point_x < origin_x_ ? 0 : start_x;
+ start_y = start_point_y < origin_y_ ? 0 : start_y;
+
+ //check end bounds
+ end_x = end_x > size_x_ ? size_x_ : end_x;
+ end_y = end_y > size_y_ ? size_y_ : end_y;
+
+ unsigned int cell_size_x = end_x - start_x;
+ unsigned int cell_size_y = end_y - start_y;
+
+ //we need a map to store the obstacles in the window temporarily
+ unsigned char local_map[cell_size_x * cell_size_y];
+
+ //copy the local window in the costmap to the local map
+ unsigned int cost_map_index = start_y * size_x_ + start_x;
+ unsigned int local_map_index = 0;
+ for(unsigned int y = 0; y < cell_size_y; ++y){
+ for(unsigned int x = 0; x < cell_size_x; ++x){
+ local_map[local_map_index] = cost_map_[cost_map_index];
+ local_map_index++;
+ cost_map_index++;
+ }
+ local_map_index += cell_size_x;
+ cost_map_index += size_x_;
+ }
+
+ //now we'll reset the costmap to the static map
+ memcpy(cost_map_, static_map_, size_x_ * size_y_);
+
+ //now we want to copy the local map back into the costmap
+ cost_map_index = start_y * size_x_ + start_x;
+ local_map_index = 0;
+ for(unsigned int y = 0; y < cell_size_y; ++y){
+ for(unsigned int x = 0; x < cell_size_x; ++x){
+ cost_map_[cost_map_index] = local_map[local_map_index];
+ local_map_index++;
+ cost_map_index++;
+ }
+ local_map_index += cell_size_x;
+ cost_map_index += size_x_;
+ }
+ }
+
+ void Costmap2D::updateObstacles(const std::vector<Observation>& obstacles){}
+
+ void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
+
+ void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
+
+ unsigned int Costmap2D::cellSizeX() const{}
+
+ unsigned int Costmap2D::cellSizeY() const{}
+
+ double Costmap2D::metersSizeX() const{}
+
+ double Costmap2D::metersSizeY() const{}
+
+ double Costmap2D::originX() const{}
+
+ double Costmap2D::originY() const{}
+
+ double Costmap2D::resolution() const{}
+};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-03-31 23:57:21
|
Revision: 13194
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13194&view=rev
Author: eitanme
Date: 2009-03-31 23:57:15 +0000 (Tue, 31 Mar 2009)
Log Message:
-----------
r19710@cib: eitan | 2009-03-20 17:34:03 -0700
More work on basic costmap structure
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19709
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19710
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-03-31 23:57:01 UTC (rev 13193)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-03-31 23:57:15 UTC (rev 13194)
@@ -38,9 +38,22 @@
#define COSTMAP_COSTMAP_2D_H_
#include <vector>
+#include <queue>
#include <new_costmap/observation.h>
+#include <robot_msgs/PointCloud.h>
namespace costmap_2d {
+ //for priority queue propagation
+ class CostmapCell {
+ public:
+ double priority;
+ unsigned int index;
+ };
+
+ bool operator<(const CostmapCell &a, const CostmapCell &b){
+ return a.priority < b.priority;
+ }
+
/**
* @class Costmap
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
@@ -96,6 +109,10 @@
*/
bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
+ inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
+ return my * size_x_ + mx;
+ }
+
/**
* @brief Convert from map coordinates to world coordinates without checking for legal bounds
* @param wx The x world coordinate
@@ -118,7 +135,7 @@
* @brief Insert new obstacles into the cost map
* @param obstacles The point clouds of obstacles to insert into the map
*/
- void updateObstacles(const std::vector<Observation>& obstacles);
+ void updateObstacles(const std::vector<Observation>& observations);
/**
* @brief Clear freespace based on any number of planar scans
@@ -185,6 +202,10 @@
double origin_y_;
unsigned char* static_map_;
unsigned char* cost_map_;
+ unsigned char* markers_;
+ std::priority_queue<CostmapCell> inflation_queue;
+ double sq_obstacle_range_;
+ double max_obstacle_height_;
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:01 UTC (rev 13193)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-03-31 23:57:15 UTC (rev 13194)
@@ -37,9 +37,11 @@
#include <new_costmap/costmap_2d.h>
using namespace std;
+using namespace robot_msgs;
namespace costmap_2d{
+
Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
double resolution, double origin_x, double origin_y) : resolution_(resolution),
origin_x_(origin_x), origin_y_(origin_y) {
@@ -108,7 +110,7 @@
unsigned char local_map[cell_size_x * cell_size_y];
//copy the local window in the costmap to the local map
- unsigned int cost_map_index = start_y * size_x_ + start_x;
+ unsigned int cost_map_index = getIndex(start_x, start_y);
unsigned int local_map_index = 0;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
@@ -121,10 +123,10 @@
}
//now we'll reset the costmap to the static map
- memcpy(cost_map_, static_map_, size_x_ * size_y_);
+ memcpy(cost_map_, static_map_, size_x_ * size_y_ * sizeof(unsigned char));
//now we want to copy the local map back into the costmap
- cost_map_index = start_y * size_x_ + start_x;
+ cost_map_index = getIndex(start_x, start_y);
local_map_index = 0;
for(unsigned int y = 0; y < cell_size_y; ++y){
for(unsigned int x = 0; x < cell_size_x; ++x){
@@ -137,8 +139,46 @@
}
}
- void Costmap2D::updateObstacles(const std::vector<Observation>& obstacles){}
+ void Costmap2D::updateObstacles(const std::vector<Observation>& observations){
+ //reset the markers for inflation
+ memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
+ //place the new obstacles into a priority queue... each with a priority of zero to begin with
+ for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
+ const Observation& obs = *it;
+ const PointCloud& cloud =*(obs.cloud_);
+ for(unsigned int i = 0; i < cloud.pts.size(); ++i){
+ //if the obstacle is too high or too far away from the robot we won't add it
+ if(cloud.pts[i].z > max_obstacle_height_)
+ continue;
+
+ //compute the squared distance from the hitpoint to the pointcloud's origin
+ double sq_dist = (cloud.pts[i].x - obs.origin_.x) * (cloud.pts[i].x - obs.origin_.x)
+ + (cloud.pts[i].y - obs.origin_.y) * (cloud.pts[i].y - obs.origin_.y)
+ + (cloud.pts[i].z - obs.origin_.z) * (cloud.pts[i].z - obs.origin_.z);
+
+ //if the point is far enough away... we won't consider it
+ if(sq_dist >= sq_obstacle_range_)
+ continue;
+
+ //now we need to compute the map coordinates for the observation
+ unsigned int mx, my;
+ if(!worldToMap(cloud.pts[i].x, cloud.pts[i].y, mx, my))
+ continue;
+
+ unsigned int index = getIndex(mx, my);
+
+ CostmapCell c;
+ c.priority = 0.0;
+ c.index = index;
+
+ //push the relevant cell index back onto the inflation queue and mark it
+ inflation_queue.push(c);
+ markers_[index] = 1;
+ }
+ }
+ }
+
void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
@@ -156,4 +196,5 @@
double Costmap2D::originY() const{}
double Costmap2D::resolution() const{}
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 01:23:15
|
Revision: 13203
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13203&view=rev
Author: eitanme
Date: 2009-04-01 01:23:08 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r19857@cib: eitan | 2009-03-23 15:10:49 -0700
Wrote part of obstacle inflation
Modified Paths:
--------------
pkg/trunk/3rdparty/trex/Makefile
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/dynamic_loader_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/dynamic_loader_controller.cpp
pkg/trunk/demos/door_demos/launch/2dnav_pr2_door.launch
pkg/trunk/demos/door_demos/launch/base_trajectory_controller.launch
pkg/trunk/demos/door_demos/launch/move_base_door.launch
pkg/trunk/demos/plug_in/manifest.xml
pkg/trunk/demos/plug_in/move_to_pickup.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/CMakeLists.txt
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/door_domain_constraints.hh
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ros_action_adapter.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseStateAdapter.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/Executive.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ROSAdapter.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/door_domain_constraints.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/main.cc
pkg/trunk/highlevel/highlevel_controllers/src/move_base_door.cpp
pkg/trunk/highlevel/robot_actions/src/action_runner.cpp
pkg/trunk/highlevel/test_executive_trex_pr2/CMakeLists.txt
pkg/trunk/mapping/door_handle_detector/CMakeLists.txt
pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/test/trigger_handle.cpp
pkg/trunk/mapping/hallway_tracker/launch/hallway_tracker.launch
pkg/trunk/mapping/hallway_tracker/manifest.xml
pkg/trunk/mapping/hallway_tracker/src/hallway_tracker.cpp
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/angles.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/distances.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/point.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/projections.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/geometry/transforms.h
pkg/trunk/mapping/point_cloud_mapping/manifest.xml
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/distances.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_parallel_lines.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/include/sbpl_door_planner/environment_navxythetadoor.h
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/src/discrete_space_information/navxythetadoor/environment_navxythetadoor.cpp
pkg/trunk/motion_planning/planning_research/sbpl_door_planner/src/test/main.cpp
pkg/trunk/nav/people_aware_nav/CMakeLists.txt
pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/head_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/prf.xacro.xml
pkg/trunk/util/laser_scan/include/laser_scan/scan_shadows_filter.h
pkg/trunk/util/laser_scan/median_filter_5.xml
pkg/trunk/util/laser_scan/src/generic_laser_filter_node.cpp
pkg/trunk/util/trajectory/manifest.xml
pkg/trunk/vision/calonder_descriptor/CMakeLists.txt
pkg/trunk/vision/outlet_detection/src/outlet_node.cpp
pkg/trunk/vision/outlet_detection/src/plug_node.cpp
pkg/trunk/vision/vslam/manifest.xml
pkg/trunk/vision/vslam/scripts/mkplot.py
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
pkg/trunk/world_models/topological_map/CMakeLists.txt
pkg/trunk/world_models/topological_map/src/tm_driver.cpp
pkg/trunk/world_models/topological_map/src/topological_map.cpp
Added Paths:
-----------
pkg/trunk/3rdparty/svl/Makefile
pkg/trunk/3rdparty/svl/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19760
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19857
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Added: pkg/trunk/3rdparty/svl/Makefile
===================================================================
--- pkg/trunk/3rdparty/svl/Makefile (rev 0)
+++ pkg/trunk/3rdparty/svl/Makefile 2009-04-01 01:23:08 UTC (rev 13203)
@@ -0,0 +1,33 @@
+INSTALL_DIR = opencv
+
+all: installed
+
+PKG_NAME = svl-src-1.1-1564
+TARBALL = build/$(PKG_NAME).tar.gz
+TARBALL_URL = http://ai.stanford.edu/~sgould/svl/media/$(PKG_NAME).tar.gz
+UNPACK_CMD = tar -xvzf
+SOURCE_DIR = build/$(PKG_NAME)
+
+include $(shell rospack find mk)/download_unpack_build.mk
+
+installed: wiped $(SOURCE_DIR)/unpacked
+ echo "Building STAIR Vision Library..."
+ # it seems that SVL does not build with the wxGTK included in ubuntu 8.04, os we are compiling this version specially for it
+ (cd build && wget "http://prdownloads.sourceforge.net/wxwindows/wxGTK-2.8.7.tar.gz" && tar xzvf wxGTK-2.8.7.tar.gz && rm wxGTK-2.8.7.tar.gz && cd wxGTK-2.8.7 && mkdir buildgtk && cd buildgtk && ../configure --disable-shared --with-opengl --enable-monolithic && make)
+ ln -sf ../../wxGTK-2.8.7 $(SOURCE_DIR)/external/wxGTK
+ ln -sf $(shell rospack find opencv_latest)/opencv $(SOURCE_DIR)/external
+ (cd $(SOURCE_DIR) && make)
+ (cd build && ln -sf $(PKG_NAME) svl)
+ (cd $(SOURCE_DIR)/external/lbfgs && ln -sf lbfgs.a liblbfgs.a)
+ (cd $(SOURCE_DIR)/external/xmlParser && ln -sf xmlParser.a libxmlParser.a)
+ touch installed
+
+wiped: Makefile
+ make wipe
+ touch wiped
+
+wipe:
+ rm -rf build installed
+
+.PHONY : clean download
+
Added: pkg/trunk/3rdparty/svl/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/svl/manifest.xml (rev 0)
+++ pkg/trunk/3rdparty/svl/manifest.xml 2009-04-01 01:23:08 UTC (rev 13203)
@@ -0,0 +1,32 @@
+<package>
+<description brief="STAIR Vision Library">
+
+ The STAIR Vision Library (SVL) (codenamed lasik) contains computer vision research code initially developed to support the STanford AI Robot project. It has since been expanded to provide a range of software infrastructure for computer vision, machine learning, and probabilistic graphical models.\
+
+</description>
+<author>Stephen Gould, Andrew Y. Ng and Daphne Koller</author>
+<license>BSD</license>
+<review status="3rdparty" notes=""/>
+<url>http://ai.stanford.edu/~sgould/svl/</url>
+<export>
+ <cpp cflags="-I${prefix}/build/svl/include -I${prefix}/build/svl/external" lflags="-L${prefix}/build/svl/bin -Wl,-rpath,${prefix}/build/svl/bin -lsvlBase -lsvlDevel -lsvlML -lsvlPGM -lsvlVision ${prefix}/build/svl/external/xmlParser/xmlParser.a ${prefix}/build/svl/external/lbfgs/lbfgs.a"/>
+</export>
+
+ <depend package="opencv_latest"/>
+ <depend package="eigen"/>
+
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libgl1-mesa-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libglu1-mesa-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libjpeg-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libgtk2.0-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libpng12-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="zlib1g-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libtiff4-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libjasper-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="python2.5-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libavcodec-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libavformat-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="ffmpeg"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
+</package>
+
Modified: pkg/trunk/demos/plug_in/move_to_pickup.py
===================================================================
--- pkg/trunk/demos/plug_in/move_to_pickup.py 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/demos/plug_in/move_to_pickup.py 2009-04-01 01:23:08 UTC (rev 13203)
@@ -132,8 +132,27 @@
rospy.set_param("cartesian_trajectory_right/pose/twist/fb_rot/d", 0.0)
rospy.set_param("cartesian_trajectory_right/pose/twist/fb_rot/i_clamp", 0.2)
+ rospy.set_param("cartesian_pose/root_name", 'torso_lift_link')
+ rospy.set_param("cartesian_pose/tip_name", 'r_gripper_tool_frame')
+ rospy.set_param("cartesian_pose/p",16.0)
+ rospy.set_param("cartesian_pose/i",4.0)
+ rospy.set_param("cartesian_pose/d",0.0)
+ rospy.set_param("cartesian_pose/i_clamp",3.0)
+
+
+ rospy.set_param("cartesian_pose/twist/ff_trans", 20.0)
+ rospy.set_param("cartesian_pose/twist/ff_rot", 5.0)
+ rospy.set_param("cartesian_pose/twist/fb_trans/p", 20.0)
+ rospy.set_param("cartesian_pose/twist/fb_trans/i", 0.5)
+ rospy.set_param("cartesian_pose/twist/fb_trans/d", 0.0 )
+ rospy.set_param("cartesian_pose/twist/fb_trans/i_clamp", 1.0)
+ rospy.set_param("cartesian_pose/twist/fb_rot/p", 1.5 )
+ rospy.set_param("cartesian_pose/twist/fb_rot/i", 0.1)
+ rospy.set_param("cartesian_pose/twist/fb_rot/d", 0.0)
+ rospy.set_param("cartesian_pose/twist/fb_rot/i_clamp", 0.2)
+
if __name__ == '__main__':
-
+
rospy.wait_for_service('spawn_controller')
rospy.init_node('move_to_pickup', anonymous = True)
rospy.wait_for_service('kill_and_spawn_controllers')
@@ -209,7 +228,9 @@
print "picking up plug"
pickup()
pub.publish(Float64(0.6))
-
+ mechanism.kill_controller('right_arm/trajectory_controller')
+ mechanism.spawn_controller(xml_for_pose.read())
+ controllers.append('cartesian_pose')
sleep(2)
#rospy.spin()
Added: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl (rev 0)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/nddl/rcs.nddl 2009-04-01 01:23:08 UTC (rev 13203)
@@ -0,0 +1,52 @@
+/*
+ * 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 <ORGANIZATION> 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.
+ */
+
+// TREX base class declaration and definitions
+#include "TREX.nddl"
+
+// Exports from sub-systems
+#include "rcs.exports.nddl"
+#include "rcs.exports.doorman.nddl"
+
+/**
+ * If an active token is dispatched, we start it immediately. Also, we just make behaviors execute quickly and
+ * flawlessly.
+ */
+Behavior::Active{
+ addEq(dispatch_time, 1, start);
+ eq(duration, 1);
+ meets(Inactive s);
+ eq(s.status, SUCCESS);
+}
+
+// Remaining behaviors for which we do not even have stubs
+ReleaseDoor release_door = new ReleaseDoor(Internal);
+OpenDoorWithoutGrasp open_door_without_grasp = new OpenDoorWithoutGrasp(Internal);
+CheckDoorway check_doorway = new CheckDoorway(Internal);
+NotifyDoorBlocked notify_door_blocked = new NotifyDoorBlocked(Internal);
Modified: pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h
===================================================================
--- pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/mapping/door_handle_detector/include/door_handle_detector/geometric_helper.h 2009-04-01 01:23:08 UTC (rev 13203)
@@ -195,6 +195,7 @@
void selectBestDistributionStatistics (const robot_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx, std::vector<int> &inliers);
void selectBestDualDistributionStatistics (const robot_msgs::PointCloud &points, const std::vector<int> &indices, int d_idx_1, int d_idx_2, std::vector<int> &inliers);
+void selectBestDualDistributionStatistics (robot_msgs::PointCloud *points, std::vector<int> *indices, int d_idx_1, int d_idx_2, std::vector<int> &inliers);
bool checkIfClusterPerpendicular (robot_msgs::PointCloud *points, std::vector<int> *indices, robot_msgs::PointStamped *viewpoint,
std::vector<double> *coeff, double eps_angle);
Modified: pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/mapping/door_handle_detector/src/doors_detector.cpp 2009-04-01 01:23:08 UTC (rev 13203)
@@ -69,6 +69,7 @@
node_->param ("~rectangle_constrain_edge_angle", rectangle_constrain_edge_angle_, 10.0);
rectangle_constrain_edge_angle_ = angles::from_degrees (rectangle_constrain_edge_angle_);
}
+#include <angles/angles.h>
// ---[ Parameters regarding optimizations / real-time computations
leaf_width_ = 0.02; // 2cm box size by default
@@ -109,7 +110,9 @@
global_marker_id_ = 1;
}
+ double minimum_z_, maximum_z_;;
+ int global_marker_id_;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -207,6 +210,8 @@
{
cloud_regions.pts.push_back (cloud_down.pts[clusters[cc][j]]);
cloud_regions.chan[0].vals.push_back (getRGB (r, g, b));
+
+ global_marker_id_ = 1;
}
}
node_->publish ("~door_regions", cloud_regions);
@@ -406,6 +411,7 @@
else
pmap_.polygons[cc].points.resize (0);
}
+ if (bad_candidate) continue;
ROS_INFO (" - Found %d / %d potential door candidates.", doors_found_cnt, (int)clusters.size ());
result.resize(doors_found_cnt);
@@ -487,7 +493,6 @@
-
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Service call to detect doors*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp 2009-04-01 01:23:08 UTC (rev 13203)
@@ -544,6 +544,10 @@
sort (r.begin (), r.end ());
r.erase (unique (r.begin (), r.end ()), r.end ());
+
+ sort (r.begin (), r.end ());
+ r.erase (unique (r.begin (), r.end ()), r.end ());
+
clusters.push_back (r);
}
}
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector.cpp 2009-04-01 01:23:08 UTC (rev 13203)
@@ -273,6 +273,20 @@
return (true);
}
+ // Go over each cluster, fit vertical lines to get rid of the points near the door edges in an elegant manner,
+ // then consider the rest as true handle candidate clusters
+ vector<vector<int> > line_inliers (clusters.size ());
+ vector<vector<int> > inliers (clusters.size ());
+ for (int i = 0; i < (int)clusters.size (); i++)
+ {
+ // One method to prune point clusters would be to fit vertical lines (Z parallel) and remove their inliers
+#if 0
+ //fitSACOrientedLine (&cloud_tr_, clusters[i], sac_distance_threshold_, &z_axis_, euclidean_cluster_angle_tolerance_, line_inliers[i]);
+ //set_difference (clusters[i].begin (), clusters[i].end (), line_inliers[i].begin (), line_inliers[i].end (),
+ // inserter (remaining_clusters[i], remaining_clusters[i].begin ()));
+#endif
+ // Grow the current cluster using the door outliers
+ growCurrentCluster (&cloud_tr_, outliers, &clusters[i], inliers[i], 2 * euclidean_cluster_distance_tolerance_);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDetector::refineHandleCandidatesWithDoorOutliers (vector<int> &handle_indices, vector<int> &outliers,
@@ -444,6 +458,31 @@
}
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Select the door outliers that could represent a handle
+ *
+ * \param indices a pointer to all the point indices
+ * \param inliers a pointer to the point indices which are inliers for the door plane
+ * \param coeff the door planar coefficients
+ * \param polygon the polygonal bounds of the door
+ * \param polygon_tr the polygonal bounds of the door in the X-Y plane
+ * \param transformation the transformation between the door planar coefficients and the X-Y plane
+ *
+ * \param outliers the resultant outliers
+ *
+ * \note The following global parameters are used:
+ * cloud_tr_, viewpoint_cloud_
+ * distance_from_door_margin_, euclidean_cluster_distance_tolerance_, euclidean_cluster_min_pts_
+ */
+ void
+ getDoorOutliers (vector<int> *indices, vector<int> *inliers,
+ vector<double> *coeff, Polygon3D *polygon, Polygon3D *polygon_tr, Eigen::Matrix4d transformation,
+ vector<int> &outliers)
+ {
+ vector<int> tmp_indices; // Used as a temporary indices array
+ Point32 tmp_p; // Used as a temporary point
+ set_difference (indices->begin (), indices->end (), inliers->begin (), inliers->end (),
+ inserter (outliers, outliers.begin ()));
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void HandleDetector::getDoorOutliers (const vector<int> &indices, const vector<int> &inliers,
@@ -521,6 +560,8 @@
while ((int)num_clouds_received_ < 1)
tictoc.sleep ();
delete message_notifier;
+ tmp_indices.resize (nr_p);
+ outliers = tmp_indices;
return detectHandle(req.door, pointcloud_, resp.doors);
}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 2009-04-01 01:23:08 UTC (rev 13203)
@@ -39,4 +39,4 @@
<property name="cal_head_tilt_gearing" value="0.970987" />
-</root>
\ No newline at end of file
+</root>==== ORIGINAL VERSION robot_descriptions/pr2/pr2_defs/calibration/prf_cal.xml 123854898574114
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:23:08 UTC (rev 13203)
@@ -43,15 +43,23 @@
#include <robot_msgs/PointCloud.h>
namespace costmap_2d {
+ static const unsigned char NO_INFORMATION = 255;
+ static const unsigned char LETHAL_OBSTACLE = 254;
+ static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
+
//for priority queue propagation
- class CostmapCell {
+ class CellData {
public:
- double priority;
- unsigned int index;
+ CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : distance_(d),
+ index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
+ double distance_;
+ unsigned int index_;
+ unsigned int x_, y_;
+ unsigned int src_x_, src_y_;
};
- bool operator<(const CostmapCell &a, const CostmapCell &b){
- return a.priority < b.priority;
+ bool operator<(const CellData &a, const CellData &b){
+ return a.distance_ < b.distance_;
}
/**
@@ -113,6 +121,39 @@
return my * size_x_ + mx;
}
+ inline void updateCellCost(unsigned int index, unsigned char cost){
+ unsigned char* cell_cost = &cost_map_[index];
+ if(*cell_cost == NO_INFORMATION)
+ *cell_cost = cost;
+ else
+ *cell_cost = std::max(cost, *cell_cost);
+ }
+
+ inline unsigned char computeCost(double distance) const {
+ unsigned char cost = 0;
+ if(distance == 0)
+ cost = LETHAL_OBSTACLE;
+ else if(distance <= inscribed_radius_)
+ cost = INSCRIBED_INFLATED_OBSTACLE;
+ else {
+ double factor = weight_ / (1 + pow(distance - inscribed_radius_, 2));
+ cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+ }
+ return cost;
+ }
+
+ inline char costLookup(int mx, int my, int src_x, int src_y){
+ unsigned int dx = abs(mx - src_x);
+ unsigned int dy = abs(my - src_y);
+ return cached_costs_[dx][dy];
+ }
+
+ inline double distanceLookup(int mx, int my, int src_x, int src_y){
+ unsigned int dx = abs(mx - src_x);
+ unsigned int dy = abs(my - src_y);
+ return cached_distances_[dx][dy];
+ }
+
/**
* @brief Convert from map coordinates to world coordinates without checking for legal bounds
* @param wx The x world coordinate
@@ -195,6 +236,43 @@
double resolution() const;
private:
+ /**
+ * @brief Given an index of a cell in the costmap, place it into a priority queue for obstacle inflation
+ * @param index The index of the cell
+ * @param mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
+ * @param my The y coordinate of the cell (can be computed from the index, but saves time to store it)
+ * @param src_x The x index of the obstacle point inflation started at
+ * @param src_y The y index of the obstacle point inflation started at
+ * @param inflation_queue The priority queue to insert into
+ * @return
+ */
+ inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
+ unsigned int src_x, unsigned int src_y, std::priority_queue<CellData*>& inflation_queue){
+ unsigned char* marked = &markers_[index];
+ //set the cost of the cell being inserted
+ if(*marked == 0){
+ //we compute our distance table one cell further than the inflation radius dictates so we can make the check below
+ double distance = distanceLookup(mx, my, src_x, src_y);
+
+ //we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
+ if(distance > inflation_radius_)
+ return;
+
+ //assign the cost associated with the distance from an obstacle to the cell
+ cost_map_[index] = costLookup(mx, my, src_x, src_y);
+
+ //push the cell data onto the queue and mark
+ inflation_queue.push(new CellData(distance, index, mx, my, src_x, src_y));
+ *marked = 1;
+ }
+ }
+
+ /**
+ * @brief Given a priority queue with the actual obstacles, compute the inflated costs for the costmap
+ * @param inflation_queue A priority queue contatining the cell data for the actual obstacles
+ */
+ void inflateObstacles(std::priority_queue<CellData*>& inflation_queue);
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
@@ -203,9 +281,12 @@
unsigned char* static_map_;
unsigned char* cost_map_;
unsigned char* markers_;
- std::priority_queue<CostmapCell> inflation_queue;
double sq_obstacle_range_;
double max_obstacle_height_;
+ unsigned char** cached_costs_;
+ double** cached_distances_;
+ double inscribed_radius_, circumscribed_radius, inflation_radius_;
+ double weight_;
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 00:59:50 UTC (rev 13202)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:23:08 UTC (rev 13203)
@@ -143,6 +143,9 @@
//reset the markers for inflation
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
+ //create a priority queue
+ std::priority_queue<CellData*> inflation_queue;
+
//place the new obstacles into a priority queue... each with a priority of zero to begin with
for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
const Observation& obs = *it;
@@ -168,17 +171,41 @@
unsigned int index = getIndex(mx, my);
- CostmapCell c;
- c.priority = 0.0;
- c.index = index;
-
- //push the relevant cell index back onto the inflation queue and mark it
- inflation_queue.push(c);
- markers_[index] = 1;
+ //push the relevant cell index back onto the inflation queue
+ enqueue(index, mx, my, mx, my, inflation_queue);
}
}
+ inflateObstacles(inflation_queue);
}
+ void Costmap2D::inflateObstacles(priority_queue<CellData*>& inflation_queue){
+ CellData* current_cell = NULL;
+ while(!inflation_queue.empty()){
+ //get the highest priority cell and pop it off the priority queue
+ current_cell = inflation_queue.top();
+ inflation_queue.pop();
+
+ unsigned int index = current_cell->index_;
+ unsigned int mx = current_cell->x_;
+ unsigned int my = current_cell->y_;
+ unsigned int sx = current_cell->src_x_;
+ unsigned int sy = current_cell->src_y_;
+
+ //attempt to put the neighbors of the current cell onto the queue
+ if(mx > 0)
+ enqueue(index - 1, mx - 1, my, sx, sy, inflation_queue);
+ if(my > 0)
+ enqueue(index - size_x_, mx, my - 1, sx, sy, inflation_queue);
+ if(mx < size_x_ - 1)
+ enqueue(index + 1, mx + 1, my, sx, sy, inflation_queue);
+ if(my < size_y_ - 1)
+ enqueue(index + size_x_, mx, my + 1, sx, sy, inflation_queue);
+
+ //delete the current_cell b/c it is no longer on the queue and no longer needed
+ delete current_cell;
+ }
+ }
+
void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 01:24:13
|
Revision: 13204
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13204&view=rev
Author: eitanme
Date: 2009-04-01 01:24:04 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r19889@cib: eitan | 2009-03-23 18:32:05 -0700
Getting closer
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19857
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19889
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:23:08 UTC (rev 13203)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:24:04 UTC (rev 13204)
@@ -86,9 +86,20 @@
* @param resolution The resolution of the map in meters/cell
* @param origin_x The x origin of the map
* @param origin_y The y origin of the map
+ * @param inscribed_radius The inscribed radius of the robot
+ * @param circumscribed_radius The circumscribed radius of the robot
+ * @param inflation_radius How far out to inflate obstacles
+ * @param obstacle_range The maximum range at which obstacles will be put into the costmap
+ * @param max_obstacle_height The maximum height of obstacles that will be considered
+ * @param weight The scaling factor for the cost function. Should be 0 < weight <= 1. Lower values reduce effective cost.
+ * @param static_data Data used to initialize the costmap
+ * @param lethal_threshold The cost threshold at which a point in the static data is considered a lethal obstacle
*/
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
- double resolution, double origin_x, double origin_y);
+ double resolution, double origin_x, double origin_y, double inscribed_radius,
+ double circumscribed_radius, double inflation_radius, double obstacle_range,
+ double max_obstacle_height, double weight,
+ const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
* @brief Get the cost of a cell in the costmap
@@ -121,6 +132,11 @@
return my * size_x_ + mx;
}
+ inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
+ my = index / size_x_;
+ mx = index - (my * size_x_);
+ }
+
inline void updateCellCost(unsigned int index, unsigned char cost){
unsigned char* cell_cost = &cost_map_[index];
if(*cell_cost == NO_INFORMATION)
@@ -273,6 +289,8 @@
*/
void inflateObstacles(std::priority_queue<CellData*>& inflation_queue);
+ unsigned int cellDistance(double world_dist);
+
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
@@ -285,7 +303,7 @@
double max_obstacle_height_;
unsigned char** cached_costs_;
double** cached_distances_;
- double inscribed_radius_, circumscribed_radius, inflation_radius_;
+ unsigned int inscribed_radius_, circumscribed_radius_, inflation_radius_;
double weight_;
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:23:08 UTC (rev 13203)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:24:04 UTC (rev 13204)
@@ -41,21 +41,75 @@
namespace costmap_2d{
-
Costmap2D::Costmap2D(double meters_size_x, double meters_size_y,
double resolution, double origin_x, double origin_y) : resolution_(resolution),
origin_x_(origin_x), origin_y_(origin_y) {
//make sure that we have a legal sized cost_map
ROS_ASSERT_MSG(meters_size_x > 0 && meters_size_y > 0, "Both the x and y dimensions of the costmap must be greater than 0.");
- size_x_ = (unsigned int) (meters_size_x / resolution);
- size_y_ = (unsigned int) (meters_size_y / resolution);
+ size_x_ = (unsigned int) ceil(meters_size_x / resolution);
+ size_y_ = (unsigned int) ceil(meters_size_y / resolution);
}
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
- double resolution, double origin_x, double origin_y) : size_x_(cells_size_x),
- size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y) {}
+ double resolution, double origin_x, double origin_y, double inscribed_radius,
+ double circumscribed_radius, double inflation_radius, double obstacle_range,
+ double max_obstacle_height, double weight,
+ const std::vector<unsigned char>& static_data, unsigned char lethal_threshold) : size_x_(cells_size_x),
+ size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
+ cost_map_(NULL), markers_(NULL), sq_obstacle_range_(obstacle_range * obstacle_range),
+ max_obstacle_height_(max_obstacle_height), cached_costs_(NULL), cached_distances_(NULL),
+ inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
+ weight_(weight){
+ //convert our inflations from world to cell distance
+ inscribed_radius_ = cellDistance(inscribed_radius);
+ circumscribed_radius_ = cellDistance(circumscribed_radius);
+ inflation_radius_ = cellDistance(inflation_radius);
+ //based on the inflation radius... compute distance and cost caches
+ cached_costs_ = new unsigned char*[inflation_radius_ + 1];
+ cached_distances_ = new double*[inflation_radius_ + 1];
+ for(unsigned int i = 0; i <= inflation_radius_; ++i){
+ cached_costs_[i] = new unsigned char[inflation_radius_ + 1];
+ cached_distances_[i] = new double[inflation_radius_ + 1];
+ for(unsigned int j = 0; i <= inflation_radius_; ++j){
+ cached_distances_[i][j] = sqrt(i*i + j*j);
+ cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
+ }
+ }
+
+ if(!static_data.empty()){
+ ROS_ASSERT_MSG(size_x_ * size_y_ == static_data_.size(), "If you want to initialize a costmap with static data, their sizes must match.");
+
+ //we'll need a priority queue for inflation
+ std::priority_queue<CellData*> inflation_queue;
+
+ //initialize the costmap with static data
+ for(unsigned int i = 0; i < size_x_; ++i){
+ for(unsigned int j = 0; j < size_y_; ++j){
+ unsigned int index = getIndex(i, j);
+ //if the static value is above the threshold... it is a lethal obstacle... otherwise just take the cost
+ cost_map_[index] = static_data[index] >= lethal_threshold ? LETHAL_OBSTACLE : static_data[index];
+ if(cost_map_[index] == LETHAL_OBSTACLE){
+ unsigned int mx, my;
+ indexToCells(index, mx, my);
+ enqueue(index, mx, my, mx, my, inflation_queue);
+ }
+ }
+ }
+ //now... let's inflate the obstacles
+ inflateObstacles(inflation_queue);
+
+ //we also want to keep a copy of the current costmap as the static map
+ memcpy(static_map_, cost_map_, size_x_ * size_y_);
+ }
+ }
+
+ unsigned int Costmap2D::cellDistance(double world_dist){
+ double cells_dist = max(0.0, ceil(world_dist / resolution_));
+ return (unsigned int) cells_dist;
+ }
+
unsigned char Costmap2D::getCellCost(unsigned int mx, unsigned int my) const {
ROS_ASSERT_MSG(mx < size_x_ && my < size_y_, "You cannot get the cost of a cell that is outside the bounds of the costmap");
return cost_map_[my * size_x_ + mx];
@@ -208,7 +262,8 @@
void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
- void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){}
+ void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){
+ }
unsigned int Costmap2D::cellSizeX() const{}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-01 02:13:46
|
Revision: 13215
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13215&view=rev
Author: eitanme
Date: 2009-04-01 02:13:41 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
New costmap merge
r20509@cib: eitan | 2009-03-31 13:25:01 -0700
Wrote a ROS node that wraps a costmap. Also, wrote observation buffers that pass information to a costmap
r20581@cib: eitan | 2009-03-31 16:46:35 -0700
Added more parameters
r20582@cib: eitan | 2009-03-31 16:48:25 -0700
Added non-templated base class for MessageNotifier to allow for common storage of notifiers listening to different message types
r20583@cib: eitan | 2009-03-31 16:55:01 -0700
Don't need to raytrace as far
Modified Paths:
--------------
pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
pkg/trunk/prcore/tf/include/tf/message_notifier.h
pkg/trunk/world_models/new_costmap/CMakeLists.txt
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/manifest.xml
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
pkg/trunk/world_models/new_costmap/src/costmap_test.cpp
Added Paths:
-----------
pkg/trunk/prcore/tf/include/tf/message_notifier_base.h
pkg/trunk/world_models/new_costmap/costmap_test.xml
pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h
pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h
pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h
pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h
pkg/trunk/world_models/new_costmap/launch_map.xml
pkg/trunk/world_models/new_costmap/src/costmap_2d_ros.cpp
pkg/trunk/world_models/new_costmap/src/observation_buffer.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20298
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:20583
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/highlevel/test_highlevel_controllers/test/launch_move_base.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -26,8 +26,8 @@
<param name="costmap_2d/inflation_radius" value="0.55"/>
<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/raytrace_range" value="10.0"/>
+ <param name="costmap_2d/raytrace_window" value="4.0"/>
+ <param name="costmap_2d/raytrace_range" value="4.0"/>
<param name="costmap_2d/weight" value="0.1"/>
<param name="costmap_2d/zLB" value="0.10"/>
<param name="costmap_2d/zUB" value="0.30"/>
Modified: pkg/trunk/prcore/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/prcore/tf/include/tf/message_notifier.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -35,6 +35,7 @@
#include <ros/node.h>
#include <tf/tf.h>
#include <tf/tfMessage.h>
+#include <tf/message_notifier_base.h>
#include <string>
#include <list>
@@ -95,7 +96,7 @@
\endverbatim
*/
template<class Message>
-class MessageNotifier
+class MessageNotifier : public MessageNotifierBase
{
public:
typedef boost::shared_ptr<Message> MessagePtr;
Added: pkg/trunk/prcore/tf/include/tf/message_notifier_base.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/message_notifier_base.h (rev 0)
+++ pkg/trunk/prcore/tf/include/tf/message_notifier_base.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,92 @@
+/*********************************************************************
+*
+* 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef MESSAGE_NOTIFIER_BASE_H_
+#define MESSAGE_NOTIFIER_BASE_H_
+#include <ros/node.h>
+#include <tf/tf.h>
+#include <tf/tfMessage.h>
+#include <tf/message_notifier_base.h>
+
+#include <string>
+#include <list>
+#include <vector>
+#include <boost/function.hpp>
+#include <boost/bind.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/thread.hpp>
+
+namespace tf {
+ /**
+ * @class MessageNotifierBase
+ * @brief An abstract base class for the various types of message notifiers
+ */
+ class MessageNotifierBase
+ {
+ public:
+
+ /**
+ * \brief Destructor
+ */
+ virtual ~MessageNotifierBase(){}
+
+ /**
+ * \brief Set the frame you need to be able to transform to before getting a message callback
+ */
+ virtual void setTargetFrame(const std::string& target_frame) = 0;
+
+ /**
+ * \brief Set the topic to listen on
+ */
+ virtual void setTopic(const std::string& topic) = 0;
+
+ /**
+ * \brief Set the required tolerance for the notifier to return true
+ */
+ virtual void setTolerance(const ros::Duration& tolerance) = 0;
+
+ /**
+ * \brief Clear any messages currently in the queue
+ */
+ virtual void clear() = 0;
+
+
+ protected:
+ MessageNotifierBase(){}
+
+ };
+};
+#endif
Modified: pkg/trunk/world_models/new_costmap/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/CMakeLists.txt 2009-04-01 02:13:41 UTC (rev 13215)
@@ -6,8 +6,8 @@
#rospack_add_boost_directories()
-rospack_add_library(new_costmap_2d src/costmap_2d.cpp)
+rospack_add_library(new_costmap_2d src/costmap_2d.cpp src/observation_buffer.cpp src/costmap_2d_ros.cpp)
#rospack_link_boost(costmap_2d thread)
-rospack_add_executable(costmap_test src/costmap_test.cpp)
+rospack_add_executable(costmap_test src/costmap_2d_ros.cpp)
target_link_libraries(costmap_test new_costmap_2d)
Added: pkg/trunk/world_models/new_costmap/costmap_test.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/costmap_test.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/costmap_test.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,12 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <node pkg="new_costmap" type="costmap_test" respawn="false" name="new_cost_map" output="screen">
+ <param name="costmap/observation_topics" value="base_scan" />
+ <!--<param name="costmap/base_scan/sensor_frame" value="base_link" />-->
+ <param name="costmap/base_scan/observation_persistance" value="0.0" />
+ <param name="costmap/base_scan/expected_update_rate" value="0.2" />
+ <param name="costmap/base_scan/data_type" value="LaserScan" />
+ </node>
+ </group>
+</launch>
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/cell_data.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,72 @@
+/*********************************************************************
+*
+* 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef COSTMAP_CELL_DATA_H_
+#define COSTMAP_CELL_DATA_H_
+namespace costmap_2d {
+ /**
+ * @class CellData
+ * @brief Storage for cell information used during obstacle inflation
+ */
+ class CellData {
+ public:
+ /**
+ * @brief Constructor for a CellData object
+ * @param d The distance to the nearest obstacle, used for ordering in the priority queue
+ * @param i The index of the cell in the cost map
+ * @param x The x coordinate of the cell in the cost map
+ * @param y The y coordinate of the cell in the cost map
+ * @param sx The x coordinate of the closest obstacle cell in the costmap
+ * @param sy The y coordinate of the closest obstacle cell in the costmap
+ * @return
+ */
+ CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : distance_(d),
+ index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
+ double distance_;
+ unsigned int index_;
+ unsigned int x_, y_;
+ unsigned int src_x_, src_y_;
+ };
+
+ /**
+ * @brief Provide an ordering between CellData objects in the priority queue
+ * @return We want the lowest distance to have the highest priority... so this returns true if a has higher priority than b
+ */
+ inline bool operator<(const CellData &a, const CellData &b){
+ return a.distance_ > b.distance_;
+ }
+};
+#endif
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/cost_values.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,45 @@
+/*********************************************************************
+*
+* 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef COSTMAP_COST_VALUES_H_
+#define COSTMAP_COST_VALUES_H_
+/** Provides a mapping for often used cost values */
+namespace costmap_2d {
+ static const unsigned char NO_INFORMATION = 255;
+ static const unsigned char LETHAL_OBSTACLE = 254;
+ static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
+};
+#endif
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:58:11 UTC (rev 13214)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -40,28 +40,11 @@
#include <vector>
#include <queue>
#include <new_costmap/observation.h>
+#include <new_costmap/cell_data.h>
+#include <new_costmap/cost_values.h>
#include <robot_msgs/PointCloud.h>
namespace costmap_2d {
- static const unsigned char NO_INFORMATION = 255;
- static const unsigned char LETHAL_OBSTACLE = 254;
- static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
-
- //for priority queue propagation
- class CellData {
- public:
- CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) : distance_(d),
- index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
- double distance_;
- unsigned int index_;
- unsigned int x_, y_;
- unsigned int src_x_, src_y_;
- };
-
- inline bool operator<(const CellData &a, const CellData &b){
- return a.distance_ > b.distance_;
- }
-
/**
* @class Costmap
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
@@ -122,6 +105,14 @@
unsigned char getCost(unsigned int mx, unsigned int my) const;
/**
+ * @brief Set the cost of a cell in the costmap
+ * @param mx The x coordinate of the cell
+ * @param my The y coordinate of the cell
+ * @param cost The cost to set the cell to
+ */
+ void setCost(unsigned int mx, unsigned int my, unsigned char cost);
+
+ /**
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d_ros.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,100 @@
+/*********************************************************************
+ *
+ * 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: Eitan Marder-Eppstein
+ *********************************************************************/
+#ifndef COSTMAP_COSTMAP_2D_ROS_H_
+#define COSTMAP_COSTMAP_2D_ROS_H_
+
+#include <ros/node.h>
+#include <ros/console.h>
+#include <new_costmap/costmap_2d.h>
+#include <new_costmap/observation_buffer.h>
+#include <robot_srvs/StaticMap.h>
+#include <robot_msgs/Polyline2D.h>
+#include <map>
+#include <vector>
+#include <string>
+#include <sstream>
+
+#include <tf/transform_datatypes.h>
+#include <tf/message_notifier_base.h>
+#include <tf/message_notifier.h>
+#include <tf/transform_listener.h>
+
+#include <laser_scan/LaserScan.h>
+#include <laser_scan/laser_scan.h>
+
+#include <robot_msgs/PointCloud.h>
+
+// Thread suppport
+#include <boost/thread.hpp>
+
+using namespace costmap_2d;
+using namespace tf;
+using namespace robot_msgs;
+
+namespace costmap_2d {
+
+ class Costmap2DROS {
+ public:
+ Costmap2DROS(ros::Node& ros_node);
+ ~Costmap2DROS();
+
+ void laserScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message, ObservationBuffer* buffer);
+ void pointCloudCallback(const tf::MessageNotifier<robot_msgs::PointCloud>::MessagePtr& message, ObservationBuffer* buffer);
+ void spin();
+ void updateMap();
+ void resetWindow();
+ void publishCostMap();
+
+ private:
+ ros::Node& ros_node_;
+ tf::TransformListener tf_; ///< @brief Used for transforming point clouds
+ laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
+ boost::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
+ Costmap2D* new_costmap_;
+ std::string global_frame_;
+ double freq_;
+ boost::thread* visualizer_thread_;
+ boost::thread* window_reset_thread_;
+
+ std::vector<tf::MessageNotifierBase*> observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
+ std::vector<ObservationBuffer*> observation_buffers_; ///< @brief Used to store observations from various sensors
+
+ };
+};
+
+#endif
+
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/observation.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,58 @@
+/*
+ * 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.
+ *
+ * Authors: Conor McGann
+ */
+
+#ifndef COSTMAP_OBSERVATION_H_
+#define COSTMAP_OBSERVATION_H_
+
+
+#include <robot_msgs/Point.h>
+#include <robot_msgs/PointCloud.h>
+
+namespace costmap_2d {
+
+ /**
+ * @brief Stores an observation in terms of a point cloud and the origin of the source
+ * @note Tried to make members and constructor arguments const but the compiler would not accept the default
+ * assignment operator for vector insertion!
+ */
+ class Observation {
+ public:
+ // Structors
+ Observation() : cloud_(){}
+ Observation(robot_msgs::Point& p, robot_msgs::PointCloud cloud): origin_(p), cloud_(cloud) {}
+ Observation(const Observation& org): origin_(org.origin_), cloud_(org.cloud_){}
+
+ robot_msgs::Point origin_;
+ robot_msgs::PointCloud cloud_;
+ };
+
+}
+#endif
Added: pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h (rev 0)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/observation_buffer.h 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,80 @@
+/*********************************************************************
+*
+* 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: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef COSTMAP_OBSERVATION_BUFFER_H_
+#define COSTMAP_OBSERVATION_BUFFER_H_
+
+#include <vector>
+#include <list>
+#include <string>
+#include <ros/time.h>
+#include <new_costmap/observation.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_datatypes.h>
+#include <robot_msgs/PointCloud.h>
+
+namespace costmap_2d {
+ /**
+ * @class ObservationBuffer
+ * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
+ */
+ class ObservationBuffer {
+ public:
+ ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
+ tf::TransformListener& tf, std::string global_frame, std::string sensor_frame);
+
+ ~ObservationBuffer();
+
+ //burden is on the user to make sure the transform is available... ie they should use a MessageNotifier
+ void bufferCloud(const robot_msgs::PointCloud& cloud);
+
+ void getObservations(std::vector<Observation>& observations);
+
+ bool isCurrent() const;
+
+ private:
+ void purgeStaleObservations();
+ tf::TransformListener& tf_;
+ const ros::Duration observation_keep_time_;
+ const ros::Duration expected_update_rate_;
+ ros::Time last_updated_;
+ std::string global_frame_;
+ std::string sensor_frame_;
+ std::list<Observation> observation_list_;
+ std::string topic_name_;
+ };
+};
+#endif
Added: pkg/trunk/world_models/new_costmap/launch_map.xml
===================================================================
--- pkg/trunk/world_models/new_costmap/launch_map.xml (rev 0)
+++ pkg/trunk/world_models/new_costmap/launch_map.xml 2009-04-01 02:13:41 UTC (rev 13215)
@@ -0,0 +1,21 @@
+<launch>
+ <master auto="start"/>
+ <group name="wg">
+ <!--
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm-fake.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ -->
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
+ <node pkg="rosstage" type="rosstage" name="rosstage" args="$(optenv ROS_STAGE_GRAPHICS -g) $(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" >
+ <param name="base_watchdog_timeout" value="0.2"/>
+ </node>
+ <node pkg="fake_localization" type="fake_localization" respawn="false" />
+ <node pkg="nav_view" type="nav_view" respawn="false"/>
+ </group>
+</launch>
Modified: pkg/trunk/world_models/new_costmap/manifest.xml
=============...
[truncated message content] |
|
From: <ei...@us...> - 2009-04-01 02:22:11
|
Revision: 13205
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13205&view=rev
Author: eitanme
Date: 2009-04-01 01:32:03 +0000 (Wed, 01 Apr 2009)
Log Message:
-----------
r19995@cib: eitan | 2009-03-24 11:27:42 -0700
Put in raytracing that should be more efficient than before and also will be compatible with rolling window costmaps
Modified Paths:
--------------
pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Modified: svk:merge
- 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19889
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
+ 637b03a7-42c1-4bbd-8d43-e365e379942c:/prfilters:13971
637b03a7-42c1-4bbd-8d43-e365e379942c:/rosTF_to_tf:9746
920d6130-5740-4ec1-bb1a-45963d5fd813:/costmap_rework_branch:19995
920d6130-5740-4ec1-bb1a-45963d5fd813:/frameidpr:7015
920d6130-5740-4ec1-bb1a-45963d5fd813:/users/josh-pr:11755
920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/josh:10136
f5854215-dd47-0410-b2c4-cdd35faa7885:/pkg/branches/rosbus:261
Modified: pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:24:04 UTC (rev 13204)
+++ pkg/trunk/world_models/new_costmap/include/new_costmap/costmap_2d.h 2009-04-01 01:32:03 UTC (rev 13205)
@@ -91,6 +91,7 @@
* @param inflation_radius How far out to inflate obstacles
* @param obstacle_range The maximum range at which obstacles will be put into the costmap
* @param max_obstacle_height The maximum height of obstacles that will be considered
+ * @param raytrace_range The maximum distance we'll raytrace out to
* @param weight The scaling factor for the cost function. Should be 0 < weight <= 1. Lower values reduce effective cost.
* @param static_data Data used to initialize the costmap
* @param lethal_threshold The cost threshold at which a point in the static data is considered a lethal obstacle
@@ -98,7 +99,7 @@
Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double obstacle_range,
- double max_obstacle_height, double weight,
+ double max_obstacle_height, double raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold);
/**
@@ -283,6 +284,50 @@
}
}
+ inline int sign(int x){
+ return x > 0 ? 1.0 : -1.0;
+ }
+
+ template <class ActionType>
+ inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1){
+ int dx = x1 - x0;
+ int dy = y1 - y0;
+
+ unsigned int abs_dx = abs(dx);
+ unsigned int abs_dy = abs(dy);
+
+ int offset_dx = sign(dx);
+ int offset_dy = sign(dy) * size_x_;
+
+ unsigned int offset = y0 * size_x_ + x0;
+
+ //if x is dominant
+ if(abs_dx >= abs_dy){
+ int error_y = abs_dx / 2;
+ bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset);
+ return;
+ }
+
+ //otherwise y is dominant
+ int error_x = abs_dy / 2;
+ bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset);
+
+ }
+
+ template <class ActionType>
+ inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
+ for(unsigned int i = 0; i < abs_da; ++i){
+ at(offset);
+ offset += offset_a;
+ error_b += abs_db;
+ if((unsigned int)error_b >= abs_da){
+ offset += offset_b;
+ error_b -= abs_da;
+ }
+ }
+ at(offset);
+ }
+
/**
* @brief Given a priority queue with the actual obstacles, compute the inflated costs for the costmap
* @param inflation_queue A priority queue contatining the cell data for the actual obstacles
@@ -301,10 +346,22 @@
unsigned char* markers_;
double sq_obstacle_range_;
double max_obstacle_height_;
+ double raytrace_range_;
unsigned char** cached_costs_;
double** cached_distances_;
unsigned int inscribed_radius_, circumscribed_radius_, inflation_radius_;
double weight_;
+
+ //functors for raytracing actions
+ class ClearCell {
+ public:
+ ClearCell(unsigned char* cost_map) : cost_map_(cost_map) {}
+ inline void operator()(unsigned int offset){
+ cost_map_[offset] = 0;
+ }
+ private:
+ unsigned char* cost_map_;
+ };
};
};
Modified: pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:24:04 UTC (rev 13204)
+++ pkg/trunk/world_models/new_costmap/src/costmap_2d.cpp 2009-04-01 01:32:03 UTC (rev 13205)
@@ -54,11 +54,11 @@
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double obstacle_range,
- double max_obstacle_height, double weight,
+ double max_obstacle_height, double raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold) : size_x_(cells_size_x),
size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
cost_map_(NULL), markers_(NULL), sq_obstacle_range_(obstacle_range * obstacle_range),
- max_obstacle_height_(max_obstacle_height), cached_costs_(NULL), cached_distances_(NULL),
+ max_obstacle_height_(max_obstacle_height), raytrace_range_(raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight){
//convert our inflations from world to cell distance
@@ -260,8 +260,77 @@
}
}
- void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){}
+ void Costmap2D::raytraceFreespace(const std::vector<Observation>& clearing_scans){
+ //pre-comput the squared raytrace range... saves a sqrt in an inner loop
+ double sq_raytrace_range = raytrace_range_ * raytrace_range_;
+
+ //create the functor that we'll use to clear cells from the costmap
+ ClearCell clearer(cost_map_);
+
+ for(unsigned int i = 0; i < clearing_scans.size(); ++i){
+ double ox = clearing_scans[i].origin_.x;
+ double oy = clearing_scans[i].origin_.y;
+ PointCloud* cloud = clearing_scans[i].cloud_;
+
+ //get the map coordinates of the origin of the sensor
+ unsigned int x0, y0;
+ if(!worldToMap(ox, oy, x0, y0))
+ return;
+
+ //we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
+ double end_x = origin_x_ + metersSizeX();
+ double end_y = origin_y_ + metersSizeY();
+
+ //for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
+ for(unsigned int j = 0; j < cloud->pts.size(); ++j){
+ double wx = cloud->pts[j].x;
+ double wy = cloud->pts[i].y;
+
+ //we want to check that we scale the vector so that we only trace to the desired distance
+ double sq_distance = (wx - ox) * (wx - ox) + (wy - oy) * (wy - oy);
+ double scaling_fact = sq_raytrace_range / sq_distance;
+ scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
+
+ //now we also need to make sure that the enpoint we're raytracing
+ //to isn't off the costmap and scale if necessary
+ double a = wx - ox;
+ double b = wy - oy;
+
+ //the minimum value to raytrace from is the origin
+ if(wx < origin_x_){
+ double t = (origin_x_ - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy < origin_y_){
+ double t = (origin_y_ - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+
+ //the maximum value to raytrace to is the end of the map
+ if(wx > end_x){
+ double t = (end_x - ox) / a;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+ if(wy > end_y){
+ double t = (end_y - oy) / b;
+ wx = ox + a * t;
+ wy = oy + b * t;
+ }
+
+ //now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
+ unsigned int x1, y1;
+ worldToMap(wx, wy, x1, y1);
+
+ //and finally... we can execute our trace to clear obstacles along that line
+ raytraceLine(clearer, x0, y0, x1, y1);
+ }
+ }
+ }
+
void Costmap2D::inflateObstaclesInWindow(double wx, double wy, double w_size_x, double w_size_y){
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X