Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[13203] pkg/trunk
|
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.
|
