Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2009-02-09 20:15:38
|
Revision: 10842
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10842&view=rev
Author: tfoote
Date: 2009-02-09 20:15:35 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
OccMap2d and MapMetaData -> robot_msgs, and StaticMap.srv -> robot_srvs
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/nav_view/manifest.xml
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view_sdl/manifest.xml
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
pkg/trunk/nav/slam_gmapping/manifest.xml
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/map_saver/manifest.xml
pkg/trunk/world_models/map_saver/src/map_saver.cpp
pkg/trunk/world_models/map_server/include/map_server/image_loader.h
pkg/trunk/world_models/map_server/manifest.xml
pkg/trunk/world_models/map_server/src/image_loader.cpp
pkg/trunk/world_models/map_server/src/main.cpp
pkg/trunk/world_models/map_server/test/rtest.cpp
pkg/trunk/world_models/map_server/test/utest.cpp
pkg/trunk/world_models/static_map_server/manifest.xml
pkg/trunk/world_models/static_map_server/nodes/map_server
pkg/trunk/world_models/static_map_server/test/consumer.py
pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_planner.h
pkg/trunk/world_models/topological_map/manifest.xml
pkg/trunk/world_models/topological_map/src/ros_topological_planner.cpp
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-09 20:15:35 UTC (rev 10842)
@@ -71,7 +71,7 @@
#include <boost/thread.hpp>
// Static map service type (for sending costmap)
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
#include <list>
@@ -190,7 +190,7 @@
/**
* @brief costmap service callback
*/
- bool costmapCallback(std_srvs::StaticMap::Request &req, std_srvs::StaticMap::Response &res);
+ bool costmapCallback(robot_srvs::StaticMap::Request &req, robot_srvs::StaticMap::Response &res);
void updateGlobalPose();
@@ -254,7 +254,7 @@
costmap_2d::CostMapAccessor* global_map_accessor_; /**< Read-only access to global cost map */
costmap_2d::CostMapAccessor* local_map_accessor_; /**< Read-only access to a window on the cost map */
- std_srvs::StaticMap::Response costmap_response_;
+ robot_srvs::StaticMap::Response costmap_response_;
tf::Stamped<tf::Pose> global_pose_; /**< The global pose in the map frame */
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -17,7 +17,7 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="robot_srvs"/>
- <depend package="std_srvs"/>
+ <!--depend package="std_srvs"/ Trying to remove Tully-->
<depend package="pr2_mechanism_controllers"/>
<depend package="pr2_msgs"/>
<depend package="ransac_ground_plane_extraction"/>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -38,7 +38,7 @@
#include <std_msgs/PointCloud.h>
#include <std_msgs/Pose2DFloat32.h>
#include <std_msgs/Polyline2D.h>
-#include <std_srvs/StaticMap.h>
+#include <robot_srvs/StaticMap.h>
#include <std_msgs/PointStamped.h>
#include <algorithm>
#include <iterator>
@@ -183,8 +183,8 @@
// get map via RPC
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
ROS_INFO("Requesting the map...\n");
while(!ros::service::call("static_map", req, resp))
{
@@ -937,7 +937,7 @@
/** Callback invoked when someone requests costmap */
- bool MoveBase::costmapCallback(std_srvs::StaticMap::Request &req, std_srvs::StaticMap::Response &res )
+ bool MoveBase::costmapCallback(robot_srvs::StaticMap::Request &req, robot_srvs::StaticMap::Response &res )
{
const unsigned char* costmap = getCostMap().getMap();
res = costmap_response_;
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-09 20:15:35 UTC (rev 10842)
@@ -101,7 +101,7 @@
#include "std_msgs/RobotBase2DOdom.h"
#include "robot_msgs/ParticleCloud.h"
#include "std_msgs/Pose2DFloat32.h"
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
// For transform support
#include "tf/transform_broadcaster.h"
@@ -239,8 +239,8 @@
playerxdr_ftable_init();
// get map via RPC
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
Modified: pkg/trunk/nav/amcl_player/manifest.xml
===================================================================
--- pkg/trunk/nav/amcl_player/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/amcl_player/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -6,7 +6,7 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="tf" />
- <depend package="std_srvs" />
+ <depend package="robot_srvs" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
</package>
Modified: pkg/trunk/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/nav_view/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -4,7 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="std_srvs"/>
+ <depend package="robot_srvs"/>
<depend package="pr2_msgs"/>
<depend package="pr2_srvs"/>
<depend package="tf"/>
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -197,8 +197,8 @@
void NavViewPanel::loadMap()
{
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
printf("Requesting the map...\n");
if( !ros::service::call("static_map", req, resp) )
{
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-02-09 20:15:35 UTC (rev 10842)
@@ -36,7 +36,7 @@
#include "robot_msgs/Planner2DGoal.h"
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
#include <OgreTexture.h>
#include <OgreMaterial.h>
Modified: pkg/trunk/nav/nav_view_sdl/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view_sdl/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/nav_view_sdl/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -4,7 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="std_srvs"/>
+ <depend package="robot_srvs"/>
<depend package="pr2_msgs"/>
<depend package="pr2_srvs"/>
<depend package="sdlgl"/>
Modified: pkg/trunk/nav/nav_view_sdl/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/nav_view_sdl/nav_view.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -102,7 +102,7 @@
#include "std_msgs/Polyline2D.h"
#include "std_msgs/Pose2DFloat32.h"
#include "pr2_msgs/OccDiff.h"
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
#include <pr2_srvs/TransientObstacles.h>
#include "sdlgl/sdlgl.h"
@@ -542,8 +542,8 @@
bool
NavView::load_map()
{
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
Modified: pkg/trunk/nav/slam_gmapping/manifest.xml
===================================================================
--- pkg/trunk/nav/slam_gmapping/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/slam_gmapping/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -6,7 +6,7 @@
<depend package="roscpp"/>
<depend package="rosconsole"/>
<depend package="std_msgs"/>
- <depend package="std_srvs"/>
+ <depend package="robot_srvs"/>
<depend package="gmapping"/>
<depend package="tf"/>
</package>
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -371,8 +371,8 @@
}
bool
-SlamGMapping::map_cb(std_srvs::StaticMap::Request &req,
- std_srvs::StaticMap::Response &res)
+SlamGMapping::map_cb(robot_srvs::StaticMap::Request &req,
+ robot_srvs::StaticMap::Response &res)
{
if(got_map_)
{
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h 2009-02-09 20:15:35 UTC (rev 10842)
@@ -31,7 +31,7 @@
#include "ros/node.h"
#include "std_msgs/LaserScan.h"
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
#include "tf/transform_listener.h"
#include "gmapping/gridfastslam/gridslamprocessor.h"
@@ -46,8 +46,8 @@
void spin() { node_->spin(); }
void laser_cb();
- bool map_cb(std_srvs::StaticMap::Request &req,
- std_srvs::StaticMap::Response &res);
+ bool map_cb(robot_srvs::StaticMap::Request &req,
+ robot_srvs::StaticMap::Response &res);
private:
ros::Node* node_;
@@ -61,7 +61,7 @@
std_msgs::LaserScan scan_;
bool got_map_;
- std_srvs::StaticMap::Response map_;
+ robot_srvs::StaticMap::Response map_;
ros::Duration map_update_interval_;
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -6,7 +6,7 @@
<depend package="roscpp" />
<depend package="player" />
<depend package="laser_scan" />
- <depend package="std_srvs" />
+ <depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="tf"/>
<export>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-09 20:15:35 UTC (rev 10842)
@@ -119,7 +119,7 @@
#include <std_msgs/PoseDot.h>
#include <std_msgs/PointCloud.h>
#include <std_msgs/LaserScan.h>
-#include <std_srvs/StaticMap.h>
+#include <robot_srvs/StaticMap.h>
// For GUI debug
#include <std_msgs/Polyline2D.h>
@@ -300,8 +300,8 @@
param("dist_penalty", dist_penalty, 2.0);
// get map via RPC
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
Added: pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg 2009-02-09 20:15:35 UTC (rev 10842)
@@ -0,0 +1,11 @@
+# The time at which the map was loaded
+time map_load_time
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad]. This is the real-world pose of the
+# cell (0,0) in the map.
+std_msgs/Pose2DFloat32 origin
\ No newline at end of file
Added: pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg 2009-02-09 20:15:35 UTC (rev 10842)
@@ -0,0 +1,16 @@
+# A 2-D grid map, in which each cell represents the probability of
+# occupancy. Occupancy values are integers in the range [0,100], or -1
+# for unknown.
+
+# The map resolution [m/cell]
+float32 resolution
+# Map width [cells]
+uint32 width
+# Map height [cells]
+uint32 height
+# The origin of the map [m, m, rad]. This is the real-world pose of the
+# cell (0,0) in the map.
+std_msgs/Pose2DFloat32 origin
+# The map data, in row-major order, starting with (0,0). Occupancy
+# probabilities are in the range [0,100]. Unknown is -1.
+byte[] data
Added: pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv (rev 0)
+++ pkg/trunk/prcore/robot_srvs/srv/StaticMap.srv 2009-02-09 20:15:35 UTC (rev 10842)
@@ -0,0 +1,2 @@
+---
+robot_msgs/OccMap2D map
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -21,9 +21,9 @@
<depend package="rxtools"/>
<depend package="planning_models"/>
<depend package="robot_msgs" />
+ <depend package="robot_srvs" />
<depend package="wxswig"/>
<depend package="wxPython_swig_interface"/>
- <depend package="std_srvs"/>
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -logre_visualizer.so"/>
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -33,7 +33,7 @@
#include "common.h"
#include <tf/transform_listener.h>
-#include <std_srvs/StaticMap.h>
+#include <robot_srvs/StaticMap.h>
#include <ogre_tools/grid.h>
@@ -181,8 +181,8 @@
void MapDisplay::load()
{
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
ROS_DEBUG("Requesting the map...");
if( !ros::service::call(service_, req, resp) )
{
@@ -407,7 +407,7 @@
const char* MapDisplay::getDescription()
{
- return "Displays an image of a map gotten through a std_srvs::StaticMap service.";
+ return "Displays an image of a map gotten through a robot_srvs::StaticMap service.";
}
} // namespace ogre_vis
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/map_display.h 2009-02-09 20:15:35 UTC (rev 10842)
@@ -35,7 +35,7 @@
#include <OgreTexture.h>
#include <OgreMaterial.h>
-#include <std_msgs/MapMetaData.h>
+#include <robot_msgs/MapMetaData.h>
#include <ros/time.h>
namespace Ogre
@@ -112,7 +112,7 @@
float alpha_;
bool new_metadata_;
- std_msgs::MapMetaData metadata_message_;
+ robot_msgs::MapMetaData metadata_message_;
ros::Time last_loaded_map_time_;
StringProperty* service_property_;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h 2009-02-09 20:15:35 UTC (rev 10842)
@@ -35,7 +35,7 @@
#include "helpers/color.h"
#include <std_msgs/Polyline2D.h>
-#include <std_msgs/MapMetaData.h>
+#include <robot_msgs/MapMetaData.h>
namespace ogre_tools
{
@@ -144,7 +144,7 @@
std_msgs::Polyline2D message_;
bool new_metadata_;
- std_msgs::MapMetaData metadata_message_;
+ robot_msgs::MapMetaData metadata_message_;
ColorProperty* color_property_;
ROSTopicStringProperty* topic_property_;
Modified: pkg/trunk/world_models/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/world_models/costmap_2d/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/costmap_2d/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -9,6 +9,7 @@
<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`"/>
</export>
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -36,7 +36,7 @@
#include <std_msgs/LaserScan.h>
#include <std_msgs/PointCloud.h>
#include <pr2_msgs/OccDiff.h>
-#include <std_srvs/StaticMap.h>
+#include <robot_srvs/StaticMap.h>
#include <pr2_srvs/TransientObstacles.h>
//Laser projection
@@ -114,8 +114,8 @@
tf_(*this, true, 1 * 1000000000ULL, 0ULL),
costmap_(WINDOW_LENGTH)
{
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
while(!ros::service::call("static_map", req, resp))
{
Modified: pkg/trunk/world_models/map_saver/manifest.xml
===================================================================
--- pkg/trunk/world_models/map_saver/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_saver/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -3,5 +3,5 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="std_srvs"/>
+ <depend package="robot_srvs"/>
</package>
Modified: pkg/trunk/world_models/map_saver/src/map_saver.cpp
===================================================================
--- pkg/trunk/world_models/map_saver/src/map_saver.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_saver/src/map_saver.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -56,7 +56,7 @@
**/
#include "ros/node.h"
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
using namespace std;
@@ -74,8 +74,8 @@
*/
MapGenerator::MapGenerator(string servname) : ros::Node("map_generator") {
puts("Requesting the map...");
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
while(!ros::service::call("static_map", req, resp))
{
printf("request '%s' failed; trying again...", servname.c_str());
Modified: pkg/trunk/world_models/map_server/include/map_server/image_loader.h
===================================================================
--- pkg/trunk/world_models/map_server/include/map_server/image_loader.h 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/include/map_server/image_loader.h 2009-02-09 20:15:35 UTC (rev 10842)
@@ -33,7 +33,7 @@
* Author: Brian Gerkey
*/
-#include "std_srvs/StaticMap.h"
+#include "robot_srvs/StaticMap.h"
namespace map_server
{
@@ -49,7 +49,7 @@
*
* @throws std::runtime_error If the image file can't be loaded
* */
-void loadMapFromFile(std_srvs::StaticMap::Response* resp,
+void loadMapFromFile(robot_srvs::StaticMap::Response* resp,
const char* fname, double res, bool negate);
}
Modified: pkg/trunk/world_models/map_server/manifest.xml
===================================================================
--- pkg/trunk/world_models/map_server/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -10,7 +10,7 @@
<review status="unreviewed" notes="API review in progress (Brian)"/>
<url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
- <depend package="std_srvs"/>
+ <depend package="robot_srvs"/>
<!-- <depend package="sdl_image"/> -->
<sysdepend os="ubuntu" version="7.04-feisty" package="libsdl-image1.2-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libsdl-image1.2-dev"/>
Modified: pkg/trunk/world_models/map_server/src/image_loader.cpp
===================================================================
--- pkg/trunk/world_models/map_server/src/image_loader.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/src/image_loader.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -51,7 +51,7 @@
{
void
-loadMapFromFile(std_srvs::StaticMap::Response* resp,
+loadMapFromFile(robot_srvs::StaticMap::Response* resp,
const char* fname, double res, bool negate)
{
SDL_Surface* img;
Modified: pkg/trunk/world_models/map_server/src/main.cpp
===================================================================
--- pkg/trunk/world_models/map_server/src/main.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/src/main.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -74,7 +74,7 @@
@section services ROS services
Offers (name/type):
-- @b "static_map"/std_srvs::StaticMap : Retrieve the map via this service
+- @b "static_map"/robot_srvs::StaticMap : Retrieve the map via this service
@section parameters ROS parameters
@@ -94,7 +94,7 @@
#include "ros/node.h"
#include "ros/publisher.h"
#include "map_server/image_loader.h"
-#include "std_msgs/MapMetaData.h"
+#include "robot_msgs/MapMetaData.h"
class MapServer : public ros::Node
{
@@ -103,8 +103,8 @@
MapServer() : ros::Node("map_server") {}
/** Callback invoked when someone requests our service */
- bool mapCallback(std_srvs::StaticMap::Request &req,
- std_srvs::StaticMap::Response &res )
+ bool mapCallback(robot_srvs::StaticMap::Request &req,
+ robot_srvs::StaticMap::Response &res )
{
// request is empty; we ignore it
@@ -116,14 +116,14 @@
/** The map response is cached here, to be sent out to service callers
*/
- std_srvs::StaticMap::Response map_resp_;
+ robot_srvs::StaticMap::Response map_resp_;
void metadataSubscriptionCallback(const ros::PublisherPtr& pub)
{
publish( "map_metadata", meta_data_message_ );
}
- std_msgs::MapMetaData meta_data_message_;
+ robot_msgs::MapMetaData meta_data_message_;
};
int main(int argc, char **argv)
Modified: pkg/trunk/world_models/map_server/test/rtest.cpp
===================================================================
--- pkg/trunk/world_models/map_server/test/rtest.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/test/rtest.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -32,7 +32,7 @@
#include <gtest/gtest.h>
#include <ros/service.h>
#include <ros/node.h>
-#include <std_srvs/StaticMap.h>
+#include <robot_srvs/StaticMap.h>
#include "test_constants.h"
@@ -65,8 +65,8 @@
{
try
{
- std_srvs::StaticMap::Request req;
- std_srvs::StaticMap::Response resp;
+ robot_srvs::StaticMap::Request req;
+ robot_srvs::StaticMap::Response resp;
// Try a few times, because the server may not be up yet.
int i=10;
bool call_result;
Modified: pkg/trunk/world_models/map_server/test/utest.cpp
===================================================================
--- pkg/trunk/world_models/map_server/test/utest.cpp 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/map_server/test/utest.cpp 2009-02-09 20:15:35 UTC (rev 10842)
@@ -43,7 +43,7 @@
{
try
{
- std_srvs::StaticMap::Response map_resp;
+ robot_srvs::StaticMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_png_file, g_valid_image_res, false);
EXPECT_FLOAT_EQ(map_resp.map.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.width, g_valid_image_width);
@@ -63,7 +63,7 @@
{
try
{
- std_srvs::StaticMap::Response map_resp;
+ robot_srvs::StaticMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, g_valid_bmp_file, g_valid_image_res, false);
EXPECT_FLOAT_EQ(map_resp.map.resolution, g_valid_image_res);
EXPECT_EQ(map_resp.map.width, g_valid_image_width);
@@ -83,7 +83,7 @@
{
try
{
- std_srvs::StaticMap::Response map_resp;
+ robot_srvs::StaticMap::Response map_resp;
map_server::loadMapFromFile(&map_resp, "foo", 0.1, false);
}
catch(std::runtime_error &e)
Modified: pkg/trunk/world_models/static_map_server/manifest.xml
===================================================================
--- pkg/trunk/world_models/static_map_server/manifest.xml 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/static_map_server/manifest.xml 2009-02-09 20:15:35 UTC (rev 10842)
@@ -8,7 +8,7 @@
<url>http://pr.willowgarage.com/</url>
<depend package="rospy" />
<depend package="std_msgs" />
- <depend package="std_srvs" />
+ <depend package="robot_srvs" />
<depend package="rostest" />
<sysdepend os="ubuntu" version="7.04-feisty" package="python-imaging"/>
Modified: pkg/trunk/world_models/static_map_server/nodes/map_server
===================================================================
--- pkg/trunk/world_models/static_map_server/nodes/map_server 2009-02-09 20:01:36 UTC (rev 10841)
+++ pkg/trunk/world_models/static_map_server/nodes/map_server 2009-02-09 20:15:35 UTC (rev 10842)
@@ -39,7 +39,7 @@
import rospy
#import std_msgs.msg
import std_msgs.msg
-import std_srvs.srv
+import robot_srvs.srv
import Image
import yaml
@@ -52,7 +52,7 @@
pass
def map_handler(self, req):
- r = std_srvs.srv.StaticMapResponse(std_msgs.msg.OccMap2D(self.scale,
+ r = robot_srvs.srv.StaticMapResponse(std_msgs.msg.OccMap2D(self.scale,
self.im.size[0],
self.im.size[1],
std_msgs.msg.Pose2DFloat32(*tuple(self.origin)),
@@ -96,7 +96,7 @@
self.origin = yaml_defs['origin']
rospy.init_node(NAME)
- s1 = rospy.Service("static_map", std_srvs.srv.StaticMap, self.map_handler)
+ s1 = rospy.Service("static_map", robot_srvs.srv.StaticMap, self.map_handler)
rospy.spin()
if __name__ == '__main__':
Modified: pkg/trunk/world_models/static_map_server/test/consumer.py
===================================================================
--- pkg/trunk/world_models/static_map_server/test/consumer.py 2009-02-09 20:01:36 UT...
[truncated message content] |
|
From: <stu...@us...> - 2009-02-09 22:55:32
|
Revision: 10847
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10847&view=rev
Author: stuglaser
Date: 2009-02-09 21:41:50 +0000 (Mon, 09 Feb 2009)
Log Message:
-----------
Removed all dependencies on stl_utils.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/hardware_interface/manifest.xml
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-09 21:41:50 UTC (rev 10847)
@@ -17,7 +17,6 @@
<depend package="std_msgs" />
<depend package="pr2_msgs" />
<depend package="robot_msgs" />
- <depend package="stl_utils" />
<depend package="angles" />
<depend package="transformations" />
<depend package="gazebo_robot_description"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp 2009-02-09 21:41:50 UTC (rev 10847)
@@ -33,7 +33,6 @@
#include <math.h>
#include <unistd.h>
#include <set>
-#include <stl_utils/stl_utils.h>
#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/Model.hh>
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2009-02-09 21:41:50 UTC (rev 10847)
@@ -37,8 +37,23 @@
#include <string>
#include <vector>
-#include <stl_utils/stl_utils.h>
+// See http://prdev.willowgarage.com/trac/personalrobots/ticket/883
+template <class C>
+void deleteElements(C *c)
+{
+ typename C::iterator it;
+ for (it = c->begin(); it != c->end(); ++it)
+ {
+ if (*it != NULL)
+ {
+ delete (*it);
+ *it = NULL;
+ }
+ }
+}
+
+
class ActuatorState{
public:
ActuatorState() :
Modified: pkg/trunk/mechanism/hardware_interface/manifest.xml
===================================================================
--- pkg/trunk/mechanism/hardware_interface/manifest.xml 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/mechanism/hardware_interface/manifest.xml 2009-02-09 21:41:50 UTC (rev 10847)
@@ -5,7 +5,6 @@
<license>BSD</license>
<review status="unreviewed" notes="API review in progress (Eric)"/>
<url>http://pr.willowgarage.com</url>
-<depend package="stl_utils" />
<export>
<cpp cflags="-I${prefix}/include"/>
</export>
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2009-02-09 21:41:50 UTC (rev 10847)
@@ -36,9 +36,9 @@
* The robot model tracks the state of the robot.
*
* State path:
- * +---------------+
+ * +---------------+
* Actuators --> | Transmissions | --> Joints --> Links
- * +---------------+
+ * +---------------+
*
* Author: Stuart Glaser
*/
@@ -49,7 +49,6 @@
#include <vector>
#include <map>
#include <string>
-#include "stl_utils/stl_utils.h"
#include "mechanism_model/link.h"
#include "mechanism_model/joint.h"
#include "mechanism_model/transmission.h"
@@ -110,7 +109,7 @@
* Each transmission refers to the actuators and joints it connects by name.
* Since name lookup is slow, for each transmission in the robot model we
* cache pointers to the actuators and joints that it connects.
- **/
+ **/
std::vector<std::vector<Actuator*> > transmissions_in_;
std::vector<std::vector<JointState*> > transmissions_out_;
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2009-02-09 21:37:38 UTC (rev 10846)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2009-02-09 21:41:50 UTC (rev 10847)
@@ -6,7 +6,6 @@
<review status="unreviewed" notes="API review in progress (Eric)"/>
<depend package="roscpp" />
<depend package="hardware_interface" />
-<depend package="stl_utils" />
<depend package="loki" />
<depend package="tinyxml" />
<depend package="wg_robot_description_parser" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-10 03:55:25
|
Revision: 10893
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10893&view=rev
Author: isucan
Date: 2009-02-10 02:36:32 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
undoing my 'fix'
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/util/self_watch/manifest.xml
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/manifest.xml
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Removed Paths:
-------------
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
pkg/trunk/world_models/robot_models/planning_models/src/urdf.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -185,13 +185,13 @@
return true;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
if (m_kmodel)
{
std::vector<std::string> links;
- planning_models::URDF::Group *g = file->getGroup("collision_check");
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
m_collisionSpace->lock();
@@ -218,9 +218,9 @@
protected:
- void addSelfCollisionGroups(unsigned int cid, planning_models::URDF *model)
+ void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
{
- std::vector<planning_models::URDF::Group*> groups;
+ std::vector<robot_desc::URDF::Group*> groups;
model->getGroups(groups);
m_collisionSpace->lock();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -43,7 +43,7 @@
#include <ros/time.h>
#include <ros/console.h>
-#include <planning_models/urdf.h>
+#include <urdf/URDF.h>
#include <planning_models/kinematic.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
@@ -130,7 +130,7 @@
void setRobotDescriptionFromData(const char *data)
{
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadString(data))
setRobotDescription(file);
else
@@ -139,14 +139,14 @@
void setRobotDescriptionFromFile(const char *filename)
{
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadFile(filename))
setRobotDescription(file);
else
delete file;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
if (m_urdf)
delete m_urdf;
@@ -296,7 +296,7 @@
ros::Node *m_node;
tf::TransformListener m_tf;
- planning_models::URDF *m_urdf;
+ robot_desc::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
// info about the pose; this is not placed in the robot's kinematic state
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2009-02-10 02:36:32 UTC (rev 10893)
@@ -13,6 +13,7 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
+ <depend package="wg_robot_description_parser"/>
<depend package="planning_models"/>
<depend package="collision_space"/>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -389,7 +389,7 @@
return result;
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
defaultPosition();
@@ -688,12 +688,12 @@
void createMotionPlanningInstances(RKPModel* model)
{
std::map<std::string, std::string> options;
- planning_models::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
+ robot_desc::URDF::Group *group = m_urdf->getGroup(model->kmodel->getURDFGroup(model->groupName));
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "RRT");
}
@@ -703,7 +703,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "LazyRRT");
}
model->addLazyRRT(options);
@@ -711,7 +711,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "EST");
}
model->addEST(options);
@@ -719,7 +719,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "SBL");
}
model->addSBL(options);
@@ -727,7 +727,7 @@
options.clear();
if (group)
{
- const planning_models::URDF::Map &data = group->data;
+ const robot_desc::URDF::Map &data = group->data;
options = data.getMapTagValues("planning", "IKSBL");
}
model->addIKSBL(options);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -200,7 +200,7 @@
}
}
- virtual void setRobotDescription(planning_models::URDF *file)
+ virtual void setRobotDescription(robot_desc::URDF *file)
{
CollisionSpaceMonitor::setRobotDescription(file);
Modified: pkg/trunk/util/self_watch/manifest.xml
===================================================================
--- pkg/trunk/util/self_watch/manifest.xml 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/util/self_watch/manifest.xml 2009-02-10 02:36:32 UTC (rev 10893)
@@ -8,6 +8,7 @@
<depend package="robot_msgs" />
+ <depend package="wg_robot_description_parser" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -66,7 +66,7 @@
if (getParam("robot_description", content))
{
// parse the description
- planning_models::URDF *file = new planning_models::URDF();
+ robot_desc::URDF *file = new robot_desc::URDF();
if (file->loadString(content.c_str()))
{
// create a kinematic model out of the parsed description
@@ -97,7 +97,7 @@
// get the list of links that are enabled for collision checking
std::vector<std::string> links;
- planning_models::URDF::Group *g = file->getGroup("collision_check");
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
if (g && g->hasFlag("collision"))
links = g->linkNames;
@@ -117,7 +117,7 @@
// get the self collision groups and add them to the collision space
int nscgroups = 0;
- std::vector<planning_models::URDF::Group*> groups;
+ std::vector<robot_desc::URDF::Group*> groups;
file->getGroups(groups);
for (unsigned int i = 0 ; i < groups.size() ; ++i)
if (groups[i]->hasFlag("self_collision"))
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -98,7 +98,7 @@
urdf_->clear();
urdf_->loadString(content.c_str());
- kinematic_model_->build( content );
+ kinematic_model_->build( *urdf_ );
kinematic_model_->defaultState();
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp 2009-02-10 02:36:32 UTC (rev 10893)
@@ -163,7 +163,7 @@
delete kinematic_model_;
kinematic_model_ = new planning_models::KinematicModel();
kinematic_model_->setVerbose( false );
- kinematic_model_->build( description_param_ );
+ kinematic_model_->build( file );
kinematic_model_->reduceToRobotFrame();
robot_->update( kinematic_model_, target_frame_ );
Modified: pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/CMakeLists.txt 2009-02-10 02:36:32 UTC (rev 10893)
@@ -2,7 +2,7 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(planning_models)
rospack_add_boost_directories()
-rospack_add_library(planning_models src/urdf.cpp src/kinematic.cpp)
+rospack_add_library(planning_models src/kinematic.cpp)
# Unit tests
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -37,7 +37,7 @@
#ifndef KINEMATIC_ROBOT_MODEL_
#define KINEMATIC_ROBOT_MODEL_
-#include <planning_models/urdf.h>
+#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <iostream>
@@ -183,7 +183,7 @@
virtual void updateVariableTransform(const double *params) = 0;
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot) = 0;
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot) = 0;
};
/** A fixed joint */
@@ -195,7 +195,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A planar joint */
@@ -212,7 +212,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A floating joint */
@@ -229,7 +229,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
/** A prismatic joint */
@@ -247,7 +247,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
double limit[2];
@@ -268,7 +268,7 @@
virtual void updateVariableTransform(const double *params);
/** Extract the information needed by the joint given the URDF description */
- virtual void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
btVector3 axis;
btVector3 anchor;
@@ -374,7 +374,7 @@
const double* computeTransform(const double *params, int groupID = -1);
/** Extract the information needed by the joint given the URDF description */
- void extractInformation(const URDF::Link *urdfLink, Robot *robot);
+ void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
};
@@ -558,7 +558,7 @@
}
void build(const std::string &description, bool ignoreSensors = false);
- virtual void build(const URDF &model, bool ignoreSensors = false);
+ virtual void build(const robot_desc::URDF &model, bool ignoreSensors = false);
bool isBuilt(void) const;
StateParams* newStateParams(void);
@@ -617,19 +617,19 @@
private:
/** Build the needed datastructure for a joint */
- void buildChainJ(Robot *robot, Link *parent, Joint *joint, const URDF::Link *urdfLink, const URDF &model);
+ void buildChainJ(Robot *robot, Link *parent, Joint *joint, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
/** Build the needed datastructure for a link */
- void buildChainL(Robot *robot, Joint *parent, Link *link, const URDF::Link *urdfLink, const URDF &model);
+ void buildChainL(Robot *robot, Joint *parent, Link *link, const robot_desc::URDF::Link *urdfLink, const robot_desc::URDF &model);
/** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
- void constructGroupList(const URDF &model);
+ void constructGroupList(const robot_desc::URDF &model);
/* compute the parameter names */
void computeParameterNames(void);
/** Allocate a joint of appropriate type, depending on the loaded link */
- Joint* createJoint(const URDF::Link* urdfLink);
+ Joint* createJoint(const robot_desc::URDF::Link* urdfLink);
};
Deleted: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-10 01:39:35 UTC (rev 10892)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/urdf.h 2009-02-10 02:36:32 UTC (rev 10893)
@@ -1,851 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/** \author Ioan Sucan */
-
-#ifndef PLANNING_MODELS_URDF_PARSER_
-#define PLANNING_MODELS_URDF_PARSER_
-
-#include <tinyxml/tinyxml.h>
-#include <iostream>
-#include <string>
-#include <vector>
-#include <map>
-
-/**
- @mainpage
-
- @htmlinclude ../../manifest.html
-
- Universal Robot Description Format (URDF) parser. The URDF class
- is able to load a robot description from multiple files, string
- buffers, streams and produce a datastructure similar to the XML
- structure. Expression evaluation and file inclusion are performed
- if needed. Checks are performed to make sure the document is
- correct: values that are inconsistend are reported, typos in the
- names, unknown tags, etc. For more documentation, see the URDF
- description. */
-
-/** Namespace that contains the URDF parser */
-namespace planning_models
-{
- /** This class contains a complete parser for URDF documents.
- Datastructures that resemble the ones specified in the XML
- document are instantiated, easy-access pointers are computed
- (to access a datastructure from another). Checks are made to
- make sure the parsed data is correct. Since actuators,
- transmissions and controllers are potentially robot specific,
- their data is kept as XML elements.
- */
- class URDF
- {
- public:
-
- /** This class encapsulates data that can be attached to various tags in the format */
- class Map
- {
- public:
-
- Map(void)
- {
- }
-
- virtual ~Map(void)
- {
- }
-
- /** Get the set of flags used for the processed <map> tags. Each <map> tag has one flag. Multiple tags may
- have the same flag. */
- void getMapTagFlags(std::vector<std::string> &flags) const;
-
- /** Given a specific flag, retrieve the names of the <map> tags that have the given flag */
- void getMapTagNames(const std::string &flag, std::vector<std::string> &names) const;
-
- /** Given a name and a flag, retrieve the defined map (string, string) */
- std::map<std::string, std::string> getMapTagValues(const std::string &flag, const std::string &name) const;
-
- /** Given a name and a flag, retrieve the defined map (string, XML) */
- std::map<std::string, const TiXmlElement*> getMapTagXML(const std::string &flag, const std::string &name) const;
-
- /** Check whether a key under empty flag and empty name has been defined */
- bool hasDefault(const std::string &key) const;
-
- /** Retrieve the value of a key (string) under empty flag and empty name has been defined */
- std::string getDefaultValue(const std::string &key) const;
-
- /** Retrieve the value of a key (XML) under empty flag and empty name has been defined */
- const TiXmlElement* getDefaultXML(const std::string &key) const;
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- /** Add a string element to the map */
- void add(const std::string &flag, const std::string &name, const std::string &key, const std::string &value);
-
- /** Add an XML element to the map */
- void add(const std::string &flag, const std::string &name, const std::string &key, const TiXmlElement *value);
-
- Map& operator=(const Map &rhs)
- {
- m_data = rhs.m_data;
- return *this;
- }
- protected:
-
- struct Element
- {
- Element(void)
- {
- xml = NULL;
- str = NULL;
- }
-
- ~Element(void)
- {
- if (str)
- delete str;
- }
-
- std::string *str; // allocated locally
- const TiXmlElement *xml; // allocated in the XML document
- };
-
- std::map < std::string, std::map < std::string, std::map < std::string, Element > > > m_data;
- };
-
-
- /** Forward declaration of groups */
- struct Group;
-
- /** This class defines link instances. It contains instances of collision, visual, inertial and joint descriptions. */
- struct Link
- {
-
- /** Class for link geometry instances */
- struct Geometry
- {
- struct Shape
- {
- virtual ~Shape(void)
- {
- }
- };
-
- struct Sphere : public Shape
- {
- Sphere(void) : Shape()
- {
- radius = 0.0;
- }
-
- double radius;
- };
-
- struct Box : public Shape
- {
- Box(void) : Shape()
- {
- size[0] = size[1] = size[2] = 0.0;
- }
-
- double size[3];
- };
-
- struct Cylinder : public Shape
- {
- Cylinder(void) : Shape()
- {
- length = radius = 0.0;
- }
-
- double length, radius;
- };
-
- struct Mesh : public Shape
- {
- Mesh(void) : Shape()
- {
- scale[0] = scale[1] = scale[2] = 1.0;
- }
-
- std::string filename;
- double scale[3];
- };
-
- Geometry(void)
- {
- type = UNKNOWN;
- shape = NULL;
- isSet["name"] = false;
- isSet["size"] = false;
- isSet["length"] = false;
- isSet["radius"] = false;
- isSet["filename"] = false;
- isSet["scale"] = false;
- }
-
- virtual ~Geometry(void)
- {
- if (shape)
- delete shape;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- enum
- {
- UNKNOWN, SPHERE, BOX, CYLINDER, MESH
- } type;
- std::string name;
- Shape *shape;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
-
- /** Class for link joint instances (connects a link to its parent) */
- struct Joint
- {
- Joint(void)
- {
- axis[0] = axis[1] = axis[2] = 0.0;
- anchor[0] = anchor[1] = anchor[2] = 0.0;
- damping = 0.0;
- friction = 0.0;
- limit[0] = limit[1] = 0.0;
- safetyLength[0] = safetyLength[1] = 0.0;
- velocityLimit = 0.0;
- effortLimit = 0.0;
- type = UNKNOWN;
- pjointMimic = NULL;
- fMimicMult = 1.0; fMimicOffset = 0.0;
- isSet["name"] = false;
- isSet["type"] = false;
- isSet["axis"] = false;
- isSet["anchor"] = false;
- isSet["damping"] = false;
- isSet["friction"] = false;
- isSet["limit"] = false;
- isSet["safetyLengthMin"] = false;
- isSet["safetyLengthMax"] = false;
- isSet["effortLimit"] = false;
- isSet["pjointMimic"] = false;
- isSet["fMimicMult"] = false;
- isSet["fMimicOffset"] = false;
- isSet["velocityLimit"] = false;
- isSet["calibration"] = false;
- }
-
- virtual ~Joint(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- enum
- {
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
- } type;
- std::string name;
- double axis[3]; // vector describing the axis of rotation: (x,y,z)
- double anchor[3]; // point about which the axis defines the rotation: (x,y,z)
- double damping; // damping coefficient
- double friction; // friction coefficient
- double limit[2]; // the joint limits: (min, max)
- double safetyLength[2]; // the joint limits: (min, max)
- double effortLimit;
- double velocityLimit;
- std::string calibration;
- Map data;
- std::map<std::string, bool> isSet;
-
- // only valid for joints that mimic other joint's values
- Joint* pjointMimic; // if not NULL, this joint mimics pjointMimic
- double fMimicMult, fMimicOffset; // the multiplication and offset coeffs. Ie, X = mult*Y+offset
- // where Y is pjointMimic's joint value, X is this joint's value.
- };
-
- /** Class for link collision component instances */
- struct Collision
- {
- Collision(void)
- {
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- verbose = false;
- geometry = NULL;
- isSet["name"] = false;
- isSet["verbose"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- isSet["geometry"] = false;
- }
-
- virtual ~Collision(void)
- {
- if (geometry)
- delete geometry;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- bool verbose;
- double xyz[3];
- double rpy[3];
- Geometry *geometry;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- /** Class for link inertial component instances */
- struct Inertial
- {
- Inertial(void)
- {
- mass = 0.0;
- com[0] = com[1] = com[2] = 0.0;
- inertia[0] = inertia[1] = inertia[2] = inertia[3] = inertia[4] = inertia[5] = 0.0;
- isSet["name"] = false;
- isSet["mass"] = false;
- isSet["inertia"] = false;
- isSet["com"] = false;
- }
-
- virtual ~Inertial(void)
- {
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- double mass;
- /** Ixx Ixy Ixz Iyy Iyz Izz */
- double inertia[6];
- double com[3];
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- /** Class for link visual component instances */
- struct Visual
- {
- Visual(void)
- {
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- geometry = NULL;
- isSet["name"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- isSet["geometry"] = false;
- }
-
- virtual ~Visual(void)
- {
- if (geometry)
- delete geometry;
- }
-
- virtual void print(std::ostream &out = std::cout, std::string indent = "") const;
-
- std::string name;
- double xyz[3];
- double rpy[3];
- Geometry *geometry;
- Map data;
- std::map<std::string, bool> isSet;
- };
-
- Link(void)
- {
- parent = NULL;
- xyz[0] = xyz[1] = xyz[2] = 0.0;
- rpy[0] = rpy[1] = rpy[2] = 0.0;
- inertial = NULL;
- visual = NULL;
- collision = NULL;
- joint = NULL;
- isSet["name"] = false;
- isSet["parent"] = false;
- isSet["inertial"] = false;
- isSet["visual"] = false;
- isSet["collision"] = false;
- isSet["joint"] = false;
- isSet["xyz"] = false;
- isSet["rpy"] = false;
- }
-
- virtual ~Link(void)
- {
- if (inertial)
- delete inertial;
- if (visual)
- delete visual;
- if (collision)
- delete collision;
- if (joint)
- delete joint;
- }
-
- /** Check if the link instance is inside some group */
- bool...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-10 05:16:11
|
Revision: 10900
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10900&view=rev
Author: tfoote
Date: 2009-02-10 05:16:00 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
LaserScan into laser_scan package #745
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/manifest.xml
pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
pkg/trunk/calibration/laser_camera_calibration/manifest.xml
pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/scan_assembler.h
pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h
pkg/trunk/deprecated/point_cloud_utils/manifest.xml
pkg/trunk/deprecated/point_cloud_utils/src/scan_assembler.cpp
pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
pkg/trunk/drivers/laser/hokuyo_node/manifest.xml
pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg
pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
pkg/trunk/drivers/laser/laser_view/laser_view.cpp
pkg/trunk/drivers/laser/laser_view/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
pkg/trunk/hardware_test/qualification/manifest.xml
pkg/trunk/hardware_test/qualification/scripts/configure_mcbs.py
pkg/trunk/hardware_test/qualification/scripts/confirm_conf.py
pkg/trunk/hardware_test/qualification/scripts/program_mcb.py
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/slam_gmapping/manifest.xml
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.h
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h
pkg/trunk/util/laser_scan/include/laser_scan/median_filter.h
pkg/trunk/util/laser_scan/manifest.xml
pkg/trunk/util/laser_scan/src/laser_scan.cc
pkg/trunk/util/laser_scan/src/median_filter.cpp
pkg/trunk/util/laser_scan/src/median_filter_node.cpp
pkg/trunk/util/laser_scan/src/scan_intensity_filter.cpp
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
pkg/trunk/util/logsetta/carmen/carmen_logger/carmen_logger.cpp
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/laser/laser_extract.cpp
pkg/trunk/util/logsetta/manifest.xml
pkg/trunk/util/mux/manifest.xml
pkg/trunk/util/mux/mux.cpp
pkg/trunk/util/mux/switch.cpp
pkg/trunk/vision/laser_processor/laser_processor.cpp
pkg/trunk/vision/laser_processor/laser_processor.h
pkg/trunk/vision/laser_processor/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.h
pkg/trunk/visualization/pr2_dashboard/src/pr2_dashboard/reset.py
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/katana/srv/StringString.srv
pkg/trunk/util/laser_scan/msg/
pkg/trunk/util/laser_scan/msg/LaserScan.msg
Modified: pkg/trunk/3rdparty/openrave/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/openrave/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/3rdparty/openrave/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -14,6 +14,7 @@
<depend package="bullet"/>
<depend package="opende"/>
<depend package="std_msgs"/>
+ <depend package="laser_scan"/>
<depend package="roscpp"/>
<versioncontrol type="svn" url="https://openrave.svn.sourceforge.net/svnroot/openrave"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libglew-dev"/>
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-02-10 05:16:00 UTC (rev 10900)
@@ -46,7 +46,7 @@
import rospy
from roslib import rostime
-from std_msgs.msg import LaserScan
+from laser_scan.msg import LaserScan
from robot_msgs.msg import MechanismState
from checkerboard_detector.msg import ObjectDetection
import copy
Modified: pkg/trunk/calibration/laser_camera_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -11,6 +11,7 @@
<review status="unreviewed" notes=""/>
<depend package="robot_msgs"/>
<depend package="std_msgs"/>
+ <depend package="laser_scan"/>
<depend package="rosrecord"/>
<depend package="rospy"/>
<depend package="rosoct"/>
Modified: pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/scan_assembler.h
===================================================================
--- pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/scan_assembler.h 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/scan_assembler.h 2009-02-10 05:16:00 UTC (rev 10900)
@@ -38,7 +38,7 @@
#include "laser_scan/laser_scan.h"
// Messages
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "std_msgs/PointCloud.h"
namespace point_cloud_utils {
@@ -68,7 +68,7 @@
* \param scan The scan that we want to add to the point cloud
* \return Negative value on error
*/
- int addScan(const std_msgs::LaserScan& scan ) ;
+ int addScan(const laser_scan::LaserScan& scan ) ;
/**
* \brief Retrieves the current assembled point cloud
Modified: pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h
===================================================================
--- pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h 2009-02-10 05:16:00 UTC (rev 10900)
@@ -35,7 +35,7 @@
#include "boost/thread/mutex.hpp"
#include "std_msgs/PointCloud.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "point_cloud_utils/scan_assembler.h"
@@ -62,7 +62,7 @@
private:
void scansCallback() ;
- std_msgs::LaserScan scan_ ;
+ laser_scan::LaserScan scan_ ;
bool got_first_scan_ ;
bool done_getting_scans_ ;
Modified: pkg/trunk/deprecated/point_cloud_utils/manifest.xml
===================================================================
--- pkg/trunk/deprecated/point_cloud_utils/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/deprecated/point_cloud_utils/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -9,6 +9,7 @@
<review status="experimental" notes=""/>
<url>http://pr.willowgarage.com</url>
<depend package="std_msgs"/>
+<depend package="laser_scan"/>
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="bullet"/>
Modified: pkg/trunk/deprecated/point_cloud_utils/src/scan_assembler.cpp
===================================================================
--- pkg/trunk/deprecated/point_cloud_utils/src/scan_assembler.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/deprecated/point_cloud_utils/src/scan_assembler.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -54,7 +54,7 @@
point_count_ = 0 ;
}
-int ScanAssembler::addScan(const std_msgs::LaserScan& scan)
+int ScanAssembler::addScan(const laser_scan::LaserScan& scan)
{
PointCloud target_frame_cloud ; // Stores the current scan in the target frame
Modified: pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/hokuyo_node/hokuyo_node.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -70,7 +70,7 @@
- None
Publishes to (name / type):
-- @b "scan"/<a href="../../std_msgs/html/classstd__msgs_1_1LaserScan.html">std_msgs/LaserScan</a> : scan data from the laser.
+- @b "scan"/<a href="../../laser_scan/html/classstd__msgs_1_1LaserScan.html">laser_scan/LaserScan</a> : scan data from the laser.
- @b "/diagnostics"/<a href="../../robot_msgs/html/classrobot__msgs_1_1DiagnosticMessage.html">robot_msgs/DiagnosticMessage</a> : diagnostic status information.
<hr>
@@ -105,7 +105,7 @@
#include "ros/time.h"
#include "ros/common.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "self_test/self_test.h"
#include "diagnostic_updater/diagnostic_updater.h"
@@ -129,7 +129,7 @@
public:
hokuyo::Laser laser_;
- std_msgs::LaserScan scan_msg_;
+ laser_scan::LaserScan scan_msg_;
double min_ang_;
double max_ang_;
@@ -146,7 +146,7 @@
HokuyoNode() : ros::Node("hokuyo"), running_(false), count_(0), self_test_(this), diagnostic_(this)
{
- advertise<std_msgs::LaserScan>("scan", 100);
+ advertise<laser_scan::LaserScan>("scan", 100);
if (hasParam("~min_ang_degrees") && hasParam("~min_ang"))
{
Modified: pkg/trunk/drivers/laser/hokuyo_node/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/hokuyo_node/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/hokuyo_node/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -4,7 +4,7 @@
<license>LGPL</license>
<review status="API cleared" notes="Tully to code review"/>
<depend package="roscpp" />
- <depend package="std_msgs" />
+ <depend package="laser_scan" />
<depend package="self_test" />
<depend package="diagnostic_updater" />
<url>http://pr.willowgarage.com/wiki/Hokuyo_TOP-URG</url>
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -6,6 +6,7 @@
<license>BSD</license>
<review status="experimental" notes=""/>
<url>http://pr.willowgarage.com</url>
+<depend package="laser_scan"/>
<depend package="std_msgs"/>
<depend package="roscpp"/>
<depend package="tf"/>
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg 2009-02-10 05:16:00 UTC (rev 10900)
@@ -1,2 +1,2 @@
-std_msgs/LaserScan scan # The original laser scan
+laser_scan/LaserScan scan # The original laser scan
std_msgs/PoseStamped[] poses # The pose during each ray of the laserscan
\ No newline at end of file
Modified: pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp
===================================================================
--- pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/laser_scan_annotator/src/laser_scan_annotator_node.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -28,14 +28,14 @@
*/
#include "ros/node.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "tf/transform_listener.h"
#include "tf/message_notifier.h"
#include "laser_scan_annotator/LaserScanAnnotated.h"
#include "boost/thread.hpp"
using namespace std ;
-using namespace std_msgs ;
+using namespace laser_scan ;
namespace laser_scan_annotator
{
@@ -104,7 +104,7 @@
}
protected:
- std_msgs::LaserScan scan_in_ ;
+ laser_scan::LaserScan scan_in_ ;
string fixed_frame_ ;
Modified: pkg/trunk/drivers/laser/laser_view/laser_view.cpp
===================================================================
--- pkg/trunk/drivers/laser/laser_view/laser_view.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/laser_view/laser_view.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -36,7 +36,7 @@
#include <math.h>
#include <GL/gl.h>
#include "ros/node.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "sdlgl/sdlgl.h"
using namespace ros;
@@ -44,7 +44,7 @@
class LaserView : public Node, public SDLGL
{
public:
- std_msgs::LaserScan laser;
+ laser_scan::LaserScan laser;
float view_scale, view_x, view_y;
LaserView() : Node("laser_view"),
Modified: pkg/trunk/drivers/laser/laser_view/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/laser_view/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/laser_view/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -3,6 +3,6 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="std_msgs"/>
+ <depend package="laser_scan"/>
<depend package="sdlgl"/>
</package>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -8,5 +8,5 @@
<review status="unreviewed" notes=""/>
<depend package="sicktoolbox"/>
<depend package="roscpp"/>
- <depend package="std_msgs"/>
+ <depend package="laser_scan"/>
</package>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -32,7 +32,7 @@
#include <csignal>
#include <sicklms-1.0/SickLMS.hh>
#include "ros/node.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
using namespace SickToolbox;
using namespace std;
@@ -45,7 +45,7 @@
class SickNode : public ros::Node
{
public:
- std_msgs::LaserScan scan_msg;
+ laser_scan::LaserScan scan_msg;
int scan_count;
string port;
int baud;
@@ -54,7 +54,7 @@
SickNode() : ros::Node("sicklms"), scan_count(0), last_print_time(0)
{
scan_msg.header.frame_id = "base_laser";
- advertise<std_msgs::LaserScan>("scan", 1);
+ advertise<laser_scan::LaserScan>("scan", 1);
param("sicklms/port", port, string("/dev/ttyUSB0"));
param("sicklms/baud", baud, 500000);
param("sicklms/inverted", inverted, true);
Modified: pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp
===================================================================
--- pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/robot/katana/nodes/katana_client/katana_client.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -2,7 +2,7 @@
#include <stdexcept>
#include <string>
#include "ros/node.h"
-#include "std_srvs/StringString.h"
+#include "deprecated_srvs/StringString.h"
using std::vector;
using std::endl;
Added: pkg/trunk/drivers/robot/katana/srv/StringString.srv
===================================================================
--- pkg/trunk/drivers/robot/katana/srv/StringString.srv (rev 0)
+++ pkg/trunk/drivers/robot/katana/srv/StringString.srv 2009-02-10 05:16:00 UTC (rev 10900)
@@ -0,0 +1,3 @@
+string str
+---
+string str
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -35,11 +35,12 @@
#include "laser_scan/laser_scan.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "point_cloud_assembler/base_assembler_srv.h"
using namespace std_msgs;
+using namespace laser_scan;
using namespace std ;
namespace point_cloud_assembler
@@ -55,10 +56,10 @@
* \section services ROS Services
* - "~build_cloud" - Inhertited from point_cloud_assembler::BaseAssemblerSrv
*/
-class LaserScanAssemblerSrv : public BaseAssemblerSrv<std_msgs::LaserScan>
+class LaserScanAssemblerSrv : public BaseAssemblerSrv<laser_scan::LaserScan>
{
public:
- LaserScanAssemblerSrv() : BaseAssemblerSrv<std_msgs::LaserScan>("laser_scan_assembler")
+ LaserScanAssemblerSrv() : BaseAssemblerSrv<laser_scan::LaserScan>("laser_scan_assembler")
{
// ***** Set Laser Projection Method *****
param("~ignore_laser_skew", ignore_laser_skew_, true) ;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -35,7 +35,7 @@
#include "laser_scan/laser_scan.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "point_cloud_assembler/base_assembler_srv.h"
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -38,11 +38,12 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "std_msgs/PointCloud.h"
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "boost/thread.hpp"
using namespace ros;
using namespace std_msgs;
+using namespace laser_scan;
static const unsigned int last_seq = 4100 ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h 2009-02-10 05:16:00 UTC (rev 10900)
@@ -32,7 +32,6 @@
#include <ros/node.h>
#include "boost/thread/mutex.hpp"
-#include <std_msgs/LaserScan.h>
#include <std_msgs/PointCloud.h>
namespace gazebo
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_laser.h 2009-02-10 05:16:00 UTC (rev 10900)
@@ -32,7 +32,7 @@
#include <ros/node.h>
#include <boost/thread/mutex.hpp>
-#include <std_msgs/LaserScan.h>
+#include <laser_scan/LaserScan.h>
namespace gazebo
{
@@ -45,7 +45,7 @@
\brief ROS Laser Scanner Controller Plugin
This controller gathers range data from a simulated ray sensor, publishes range data through
- std_msgs::LaserScan ROS topic.
+ laser_scan::LaserScan ROS topic.
Example Usage:
\verbatim
@@ -81,7 +81,7 @@
/**
\brief ROS laser scan controller.
\li Starts a ROS node if none exists.
- \li Simulates a laser range sensor and publish std_msgs::LaserScan.msg over ROS.
+ \li Simulates a laser range sensor and publish laser_scan::LaserScan.msg over ROS.
\li Example Usage:
\verbatim
<model:physical name="ray_model">
@@ -144,7 +144,7 @@
private: ros::Node *rosnode;
/// \brief ros message
- private: std_msgs::LaserScan laserMsg;
+ private: laser_scan::LaserScan laserMsg;
/// \brief topic name
private: std::string topicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -15,6 +15,7 @@
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="std_msgs" />
+ <depend package="laser_scan" />
<depend package="pr2_msgs" />
<depend package="robot_msgs" />
<depend package="angles" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_laser.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -82,7 +82,7 @@
{
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
- rosnode->advertise<std_msgs::LaserScan>(this->topicName,10);
+ 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/hardware_test/qualification/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/qualification/manifest.xml 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/hardware_test/qualification/manifest.xml 2009-02-10 05:16:00 UTC (rev 10900)
@@ -7,7 +7,7 @@
<depend package="rospy" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
- <depend package="std_srvs" />
+ <depend package="deprecated_srvs" />
<depend package="robot_srvs" />
<depend package="hokuyo_node" />
<depend package="imu_node" />
Modified: pkg/trunk/hardware_test/qualification/scripts/configure_mcbs.py
===================================================================
--- pkg/trunk/hardware_test/qualification/scripts/configure_mcbs.py 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/hardware_test/qualification/scripts/configure_mcbs.py 2009-02-10 05:16:00 UTC (rev 10900)
@@ -39,7 +39,7 @@
import subprocess
from optparse import OptionParser
-from std_srvs.srv import *
+from deprecated_srvs.srv import *
rospy.init_node("mcb_configurer")
# block until the add_two_ints service is available
Modified: pkg/trunk/hardware_test/qualification/scripts/confirm_conf.py
===================================================================
--- pkg/trunk/hardware_test/qualification/scripts/confirm_conf.py 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/hardware_test/qualification/scripts/confirm_conf.py 2009-02-10 05:16:00 UTC (rev 10900)
@@ -42,7 +42,7 @@
import roslib
roslib.load_manifest(PKG)
import wx
-from std_srvs.srv import *
+from deprecated_srvs.srv import *
import rospy
import time
Modified: pkg/trunk/hardware_test/qualification/scripts/program_mcb.py
===================================================================
--- pkg/trunk/hardware_test/qualification/scripts/program_mcb.py 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/hardware_test/qualification/scripts/program_mcb.py 2009-02-10 05:16:00 UTC (rev 10900)
@@ -39,7 +39,7 @@
import subprocess
from optparse import OptionParser
-from std_srvs.srv import *
+from deprecated_srvs.srv import *
rospy.init_node("mcb_programmer")
# block until the add_two_ints service is available
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-10 05:16:00 UTC (rev 10900)
@@ -55,7 +55,7 @@
#include <robot_msgs/Planner2DState.h>
#include <robot_msgs/Planner2DGoal.h>
#include <std_msgs/PoseDot.h>
-#include <std_msgs/LaserScan.h>
+#include <laser_scan/LaserScan.h>
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/PointCloud.h>
@@ -174,7 +174,7 @@
/**
* @brief Callbacks for perceiving obstalces
*/
- void baseScanCallback(const tf::MessageNotifier<std_msgs::LaserScan>::MessagePtr& message);
+ void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void tiltScanCallback();
void tiltCloudCallback(const tf::MessageNotifier<std_msgs::PointCloud>::MessagePtr& message);
void groundPlaneCloudCallback();
@@ -225,8 +225,8 @@
bool checkWatchDog() const;
// Callback messages
- std_msgs::LaserScan baseScanMsg_; /**< Filled by subscriber with new base laser scans */
- std_msgs::LaserScan tiltScanMsg_; /**< Filled by subscriber with new tilte laser scans */
+ laser_scan::LaserScan baseScanMsg_; /**< Filled by subscriber with new base laser scans */
+ laser_scan::LaserScan tiltScanMsg_; /**< Filled by subscriber with new tilte laser scans */
std_msgs::PointCloud tiltCloudMsg_; /**< Filled by subscriber with new tilte laser scans */
std_msgs::PointCloud groundPlaneCloudMsg_; /**< Filled by subscriber with point clouds */
std_msgs::PointCloud stereoCloudMsg_; /**< Filled by subscriber with point clouds */
@@ -283,7 +283,7 @@
//Robot filter
robot_filter::RobotFilter* filter_;
- tf::MessageNotifier<std_msgs::LaserScan>* baseScanNotifier_;
+ tf::MessageNotifier<laser_scan::LaserScan>* baseScanNotifier_;
tf::MessageNotifier<std_msgs::PointCloud>* tiltLaserNotifier_;
//flag for reseting the costmap.
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-10 05:16:00 UTC (rev 10900)
@@ -291,7 +291,7 @@
// 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<std_msgs::LaserScan>(&tf_, this,
+ baseScanNotifier_ = new tf::MessageNotifier<laser_scan::LaserScan>(&tf_, this,
boost::bind(&MoveBase::baseScanCallback, this, _1),
"base_scan", global_frame_, 50);
tiltLaserNotifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(&tf_, this,
@@ -419,7 +419,7 @@
stateMsg.pos.th = (float)yaw;
}
- void MoveBase::baseScanCallback(const tf::MessageNotifier<std_msgs::LaserScan>::MessagePtr& message)
+ void MoveBase::baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message)
{
// Project laser into point cloud
std_msgs::PointCloud local_cloud;
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-10 03:28:10 UTC (rev 10899)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-10 05:16:00 UTC (rev 10900)
@@ -97,7 +97,7 @@
#include "ros/node.h"
// Messages that I need
-#include "std_msgs/LaserScan.h"
+#include "laser_scan/LaserScan.h"
#include "std_msgs/RobotBase2DOdom.h"
#include "robot_msgs/ParticleCloud.h"
#include "std_msgs/Pose2DFloat32.h"
@@ -147,7 +147,7 @@
std_msgs::RobotBase2DOdom localizedOdomMsg;
robot_msgs::ParticleCloud particleCloudMsg;
std_msgs::RobotBase2DOdom odomMsg;
- std_msgs::LaserScan laserMsg;
+ laser_scan::LaserScan laserMsg;
std_msgs::Pose2DFloat32 initialPoseMsg;
// Message callbacks
@@ -190,7 +190,7 @@
const ros::Time& t, const std::string& f);
// buffer of not-yet-transformed scans
- std::deque<std_msgs::LaserScan> laser_scans;
+ std::deque<laser_scan::LaserScan> laser_scans;
//time for tolerance on the published transform,
//basically defines how long a map->odom transform is good for
@@ -787,13 +787,13 @@
}
// Put it on the queue
- std_msgs::LaserScan newscan(laserMsg);
+ laser_scan::LaserScan newscan(laserMsg);
laser_scans.push_back(newscan);
// Process the queued scans
w...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-10 05:44:43
|
Revision: 10903
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10903&view=rev
Author: tfoote
Date: 2009-02-10 05:44:39 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
deprecating Image and ImageArray
Modified Paths:
--------------
pkg/trunk/deprecated/cv_wrappers/manifest.xml
pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp
pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp
pkg/trunk/deprecated/deprecated_srvs/manifest.xml
pkg/trunk/deprecated/deprecated_srvs/srv/PolledImage.srv
pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp
pkg/trunk/drivers/cam/elphel_cam/manifest.xml
pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/util/image_utils/manifest.xml
pkg/trunk/util/image_utils/test/test_image_codec/test_image_codec.cpp
pkg/trunk/vision/color_calib/calib_node.cpp
pkg/trunk/vision/color_calib/manifest.xml
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
pkg/trunk/vision/spacetime_stereo/manifest.xml
pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp
pkg/trunk/vision/spacetime_stereo/sts_node.cpp
pkg/trunk/vision/stereo_blob_tracker/include/blob_tracker.h
pkg/trunk/vision/stereo_blob_tracker/manifest.xml
pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp
pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp
pkg/trunk/vision/visual_odometry/manifest.xml
pkg/trunk/vision/visual_odometry/nodes/camview_py.py
pkg/trunk/vision/visual_odometry/nodes/corrector.py
pkg/trunk/vision/visual_odometry/nodes/lineperf.py
pkg/trunk/vision/visual_odometry/nodes/vo.py
pkg/trunk/visualization/wx_camera_panel/manifest.xml
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
Added Paths:
-----------
pkg/trunk/deprecated/deprecated_msgs/msg/ImageArray.msg
Modified: pkg/trunk/deprecated/cv_wrappers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/cv_wrappers/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -21,6 +21,7 @@
<depend package="roscpp"/>
<depend package="opencv_latest"/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<depend package="std_srvs"/>
<depend package="image_msgs"/>
<depend package="image_utils"/>
Modified: pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/cv_wrappers/minimal/cv_view.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -4,7 +4,7 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "ros/node.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "image_utils/cv_bridge.h"
using namespace std;
@@ -13,10 +13,10 @@
class CvView : public Node
{
public:
- std_msgs::Image image_msg;
- CvBridge<std_msgs::Image> cv_bridge;
+ deprecated_msgs::Image image_msg;
+ CvBridge<deprecated_msgs::Image> cv_bridge;
- CvView() : Node("cv_view"), cv_bridge(&image_msg, CvBridge<std_msgs::Image>::CORRECT_BGR)
+ CvView() : Node("cv_view"), cv_bridge(&image_msg, CvBridge<deprecated_msgs::Image>::CORRECT_BGR)
{
cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
subscribe("image", image_msg, &CvView::image_cb, 1);
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_movie_streamer.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -3,7 +3,7 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include "ros/node.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "image_utils/cv_bridge.h"
using namespace std;
@@ -27,8 +27,8 @@
class CvMovieStreamer : public Node
{
public:
- std_msgs::Image image_msg;
- CvBridge<std_msgs::Image> cv_bridge;
+ deprecated_msgs::Image image_msg;
+ CvBridge<deprecated_msgs::Image> cv_bridge;
string movie_fname;
int delay, loop, qual;
@@ -36,7 +36,7 @@
: Node("cv_movie_streamer"), cv_bridge(&image_msg),
movie_fname(_movie_fname), delay(_delay), loop(_loop), qual(_qual)
{
- advertise<std_msgs::Image>("image", 1);
+ advertise<deprecated_msgs::Image>("image", 1);
}
void stream_movie()
{
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_view.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -5,7 +5,7 @@
#include "opencv/highgui.h"
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "image_utils/cv_bridge.h"
#include <sys/stat.h>
@@ -15,8 +15,8 @@
class CvView : public ros::Node
{
public:
- std_msgs::Image image_msg;
- CvBridge<std_msgs::Image> cv_bridge;
+ deprecated_msgs::Image image_msg;
+ CvBridge<deprecated_msgs::Image> cv_bridge;
boost::mutex cv_mutex;
@@ -26,7 +26,7 @@
int img_cnt;
bool made_dir;
- CvView() : Node("cv_view", ros::Node::ANONYMOUS_NAME), cv_bridge(&image_msg, CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U),
+ CvView() : Node("cv_view", ros::Node::ANONYMOUS_NAME), cv_bridge(&image_msg, CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U),
cv_image(0), img_cnt(0), made_dir(false)
{
cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
Modified: pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp
===================================================================
--- pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/cv_wrappers/src/cv_view_array.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -6,7 +6,7 @@
#include "opencv/highgui.h"
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "std_msgs/ImageArray.h"
+#include "deprecated_msgs/ImageArray.h"
#include "image_utils/cv_bridge.h"
#include "color_calib.h"
@@ -19,14 +19,14 @@
{
string label;
IplImage *cv_image;
- CvBridge<std_msgs::Image> *bridge;
+ CvBridge<deprecated_msgs::Image> *bridge;
color_calib::Calibration* color_cal;
};
class CvView : public ros::Node
{
public:
- std_msgs::ImageArray image_msg;
+ deprecated_msgs::ImageArray image_msg;
boost::mutex cv_mutex;
@@ -75,7 +75,7 @@
if (j == images.end())
{
images[l].label = image_msg.images[i].label;
- images[l].bridge = new CvBridge<std_msgs::Image>(&image_msg.images[i], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
+ images[l].bridge = new CvBridge<deprecated_msgs::Image>(&image_msg.images[i], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
cvNamedWindow(l.c_str(), CV_WINDOW_AUTOSIZE);
images[l].cv_image = 0;
images[l].color_cal = new color_calib::Calibration(this);
Added: pkg/trunk/deprecated/deprecated_msgs/msg/ImageArray.msg
===================================================================
--- pkg/trunk/deprecated/deprecated_msgs/msg/ImageArray.msg (rev 0)
+++ pkg/trunk/deprecated/deprecated_msgs/msg/ImageArray.msg 2009-02-10 05:44:39 UTC (rev 10903)
@@ -0,0 +1,2 @@
+Header header
+Image[] images
Modified: pkg/trunk/deprecated/deprecated_srvs/manifest.xml
===================================================================
--- pkg/trunk/deprecated/deprecated_srvs/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/deprecated_srvs/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -13,6 +13,7 @@
<logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
<depend package="roslib"/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<export>
<cpp cflags="-I${prefix}/srv/cpp"/>
</export>
Modified: pkg/trunk/deprecated/deprecated_srvs/srv/PolledImage.srv
===================================================================
--- pkg/trunk/deprecated/deprecated_srvs/srv/PolledImage.srv 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/deprecated_srvs/srv/PolledImage.srv 2009-02-10 05:44:39 UTC (rev 10903)
@@ -1,2 +1,2 @@
---
-std_msgs/Image image
\ No newline at end of file
+deprecated_msgs/Image image
\ No newline at end of file
Modified: pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv
===================================================================
--- pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv 2009-02-10 05:44:39 UTC (rev 10903)
@@ -1,5 +1,5 @@
---
-std_msgs/Image image
+deprecated_msgs/Image image
std_msgs/Point32[] points
uint32[] rows
uint32[] cols
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -14,6 +14,7 @@
<url>http://stair.stanford.edu</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="deprecated_msgs"/>
<depend package="deprecated_srvs"/>
<depend package="image_utils"/>
<depend package="self_test"/>
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_demo/axis_demo.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -31,7 +31,7 @@
#include <iostream>
#include "ros/node.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "axis_cam/PTZActuatorCmd.h"
#include "axis_cam/PTZActuatorState.h"
#include "image_utils/cv_bridge.h"
@@ -44,15 +44,15 @@
class AxisDemo : public ros::Node
{
public:
- std_msgs::Image image_;
- CvBridge<std_msgs::Image> cv_bridge_;
+ deprecated_msgs::Image image_;
+ CvBridge<deprecated_msgs::Image> cv_bridge_;
axis_cam::PTZActuatorCmd ptz_cmd_;
axis_cam::PTZActuatorState ptz_state_;
IplImage *cv_image_;
- AxisDemo() : ros::Node("axis_demo"), cv_bridge_(&image_, CvBridge<std_msgs::Image>::CORRECT_BGR), cv_image_(NULL)
+ AxisDemo() : ros::Node("axis_demo"), cv_bridge_(&image_, CvBridge<deprecated_msgs::Image>::CORRECT_BGR), cv_image_(NULL)
{
cvNamedWindow("cv_view", CV_WINDOW_AUTOSIZE);
Modified: pkg/trunk/drivers/cam/elphel_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/elphel_cam/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/cam/elphel_cam/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -7,6 +7,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<depend package="image_utils"/>
<depend package="roscpp"/>
Modified: pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/cam/elphel_cam/src/elphel_node/elphel_node.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -42,7 +42,7 @@
#include "highgui.h"
#include <stdio.h>
#include <ctype.h>
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "image_utils/cv_bridge.h"
@@ -54,9 +54,9 @@
class Elphel_Node : public ros::Node
{
public:
- std_msgs::Image image_msg;
- CvBridge<std_msgs::Image> cv_bridge;
- ImageCodec<std_msgs::Image> codec;
+ deprecated_msgs::Image image_msg;
+ CvBridge<deprecated_msgs::Image> cv_bridge;
+ ImageCodec<deprecated_msgs::Image> codec;
uint8_t* jpeg;
uint32_t jpeg_size;
@@ -67,7 +67,7 @@
Elphel_Node() : ros::Node("elphel"), cv_bridge(&image_msg), codec(&image_msg)
{
- advertise<std_msgs::Image>("elphel_bus");
+ advertise<deprecated_msgs::Image>("elphel_bus");
e = new Elphel_Cam("10.12.0.103");
// e->init(10, 4, 4);
e->init(1, 4, 4);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_camera.h 2009-02-10 05:44:39 UTC (rev 10903)
@@ -29,7 +29,7 @@
#include <ros/node.h>
#include "boost/thread/mutex.hpp"
-#include <std_msgs/Image.h>
+#include <deprecated_msgs/Image.h>
#include <gazebo/Controller.hh>
namespace gazebo
@@ -42,7 +42,7 @@
\brief Ros Camera Plugin Controller.
- This is a controller that collects data from a Camera Sensor and populates a libgazebo camera interface as well as publish a ROS std_msgs::Image (under the field \b \<topicName\>). This controller should only be used as a child of a camera sensor (see example below.
+ This is a controller that collects data from a Camera Sensor and populates a libgazebo camera interface as well as publish a ROS deprecated_msgs::Image (under the field \b \<topicName\>). This controller should only be used as a child of a camera sensor (see example below.
Example Usage:
\verbatim
@@ -68,7 +68,7 @@
\brief RosCamera Controller.
\li Starts a ROS node if none exists. \n
- \li Simulates a generic camera and broadcast std_msgs::Image topic over ROS.
+ \li Simulates a generic camera and broadcast deprecated_msgs::Image topic over ROS.
\li Example Usage:
\verbatim
<model:physical name="camera_model">
@@ -120,7 +120,7 @@
private: ros::Node *rosnode;
/// \brief ROS image message
- private: std_msgs::Image imageMsg;
+ private: deprecated_msgs::Image imageMsg;
/// \brief ROS image topic name
private: std::string topicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h 2009-02-10 05:44:39 UTC (rev 10903)
@@ -33,7 +33,7 @@
#include <ros/node.h>
#include "boost/thread/mutex.hpp"
#include <std_msgs/PointCloud.h>
-#include <std_msgs/Image.h>
+#include <deprecated_msgs/Image.h>
#include <Generic_Camera.hh>
#include <gazebo/gazebo.h>
@@ -145,7 +145,7 @@
private: std_msgs::PointCloud leftCloudMsg;
private: std_msgs::PointCloud rightCloudMsg;
/// \brief ros message
- private: std_msgs::Image imageMsg[2];
+ private: deprecated_msgs::Image imageMsg[2];
/// \brief topic name
private: std::string leftCloudTopicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_time.h 2009-02-10 05:44:39 UTC (rev 10903)
@@ -33,7 +33,7 @@
#include <ros/node.h>
#include "boost/thread/mutex.hpp"
-#include <std_msgs/Image.h>
+
// roscpp - used for broadcasting time over ros
#include <roslib/Time.h>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -15,6 +15,7 @@
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
<depend package="laser_scan" />
<depend package="pr2_msgs" />
<depend package="robot_msgs" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_camera.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -103,7 +103,7 @@
this->frameName = node->GetString("frameName","default_ros_camera",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
- rosnode->advertise<std_msgs::Image>(this->topicName,1);
+ rosnode->advertise<deprecated_msgs::Image>(this->topicName,1);
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -113,8 +113,8 @@
std::cout << "================= " << this->leftCloudTopicName << std::endl;
rosnode->advertise<std_msgs::PointCloud>(this->leftCloudTopicName, 1);
rosnode->advertise<std_msgs::PointCloud>(this->rightCloudTopicName, 1);
- rosnode->advertise<std_msgs::Image>(this->leftTopicName, 1);
- rosnode->advertise<std_msgs::Image>(this->rightTopicName, 1);
+ rosnode->advertise<deprecated_msgs::Image>(this->leftTopicName, 1);
+ rosnode->advertise<deprecated_msgs::Image>(this->rightTopicName, 1);
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/util/image_utils/manifest.xml
===================================================================
--- pkg/trunk/util/image_utils/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/util/image_utils/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -12,6 +12,7 @@
<url>http://stair.stanford.edu</url>
<depend package="ijg_libjpeg"/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<depend package="roscpp"/>
<depend package="opencv_latest"/>
<export>
Modified: pkg/trunk/util/image_utils/test/test_image_codec/test_image_codec.cpp
===================================================================
--- pkg/trunk/util/image_utils/test/test_image_codec/test_image_codec.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/util/image_utils/test/test_image_codec/test_image_codec.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -30,13 +30,13 @@
#include <cstdio>
#include "image_utils/image_codec.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
int main(int argc, char **argv)
{
// todo: test runtime functionality, not just compilation.
- std_msgs::Image msg;
- ImageCodec<std_msgs::Image> codec(&msg);
+ deprecated_msgs::Image msg;
+ ImageCodec<deprecated_msgs::Image> codec(&msg);
return 0;
}
Modified: pkg/trunk/vision/color_calib/calib_node.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calib_node.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/color_calib/calib_node.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -40,7 +40,7 @@
#include "opencv/highgui.h"
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "std_msgs/ImageArray.h"
+#include "deprecated_msgs/ImageArray.h"
#include "image_utils/cv_bridge.h"
#include <sys/stat.h>
@@ -53,7 +53,7 @@
class ColorCalib : public ros::Node
{
public:
- std_msgs::ImageArray image_msg;
+ deprecated_msgs::ImageArray image_msg;
boost::mutex cv_mutex;
@@ -81,7 +81,7 @@
if (image_msg.images[i].colorspace == std::string("rgb24"))
{
- CvBridge<std_msgs::Image>* cv_bridge = new CvBridge<std_msgs::Image>(&image_msg.images[i], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
+ CvBridge<deprecated_msgs::Image>* cv_bridge = new CvBridge<deprecated_msgs::Image>(&image_msg.images[i], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
IplImage* img;
if (cv_bridge->to_cv(&img))
Modified: pkg/trunk/vision/color_calib/manifest.xml
===================================================================
--- pkg/trunk/vision/color_calib/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/color_calib/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -9,6 +9,7 @@
<url>http://www.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="deprecated_msgs"/>
<depend package="std_srvs"/>
<depend package="image_utils"/>
<depend package="filters"/>
Modified: pkg/trunk/vision/people/manifest.xml
===================================================================
--- pkg/trunk/vision/people/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/people/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -9,6 +9,7 @@
<url>http://www.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="deprecated_msgs"/>
<depend package="robot_msgs"/>
<depend package="std_srvs"/>
<depend package="opencv_latest"/>
Modified: pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_feature_tracker.py 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/people/src/stereo_face_feature_tracker.py 2009-02-10 05:44:39 UTC (rev 10903)
@@ -5,7 +5,8 @@
import face_detection
from visualodometer import VisualOdometer, FeatureDetectorStar, DescriptorSchemeCalonder, DescriptorSchemeSAD
import camera
-from std_msgs.msg import Image, ImageArray, String, PointStamped, PointCloud, Point
+from std_msgs.msg import String, PointStamped, PointCloud, Point
+from deprecated_msgs.msg import Image, ImageArray
from robot_msgs.msg import PositionMeasurement
import visual_odometry as VO
import starfeature
Modified: pkg/trunk/vision/spacetime_stereo/manifest.xml
===================================================================
--- pkg/trunk/vision/spacetime_stereo/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/spacetime_stereo/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -3,6 +3,7 @@
<license>BSD</license>
<depend package="roscpp" />
<depend package="std_msgs" />
+<depend package="deprecated_msgs" />
<depend package="opencv_latest" />
<depend package="image_utils" />
</package>
Modified: pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp
===================================================================
--- pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -71,8 +71,8 @@
#include <opencv/cxcore.h>
#include "ros/node.h"
#include <std_msgs/PointCloud.h>
-#include <std_msgs/ImageArray.h>
-#include <std_msgs/Image.h>
+#include <deprecated_msgs/ImageArray.h>
+#include <deprecated_msgs/Image.h>
#include "limits.h"
#include "image_utils/cv_bridge.h"
#include <vector>
@@ -221,11 +221,11 @@
{
public:
//IplImage *frame;
- std_msgs::ImageArray frame_msg;
+ deprecated_msgs::ImageArray frame_msg;
std_msgs::String cal_msg;
bool builtBridge;
- CvBridge<std_msgs::Image> *left_bridge_in;
- CvBridge<std_msgs::Image> *right_bridge_in;
+ CvBridge<deprecated_msgs::Image> *left_bridge_in;
+ CvBridge<deprecated_msgs::Image> *right_bridge_in;
//boost::mutex frame_mutex_;
bool IsCal;
@@ -267,8 +267,8 @@
{
if(!builtBridge) {
- left_bridge_in = new CvBridge<std_msgs::Image>(&frame_msg.images[1], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
- right_bridge_in = new CvBridge<std_msgs::Image>(&frame_msg.images[0], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
+ left_bridge_in = new CvBridge<deprecated_msgs::Image>(&frame_msg.images[1], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
+ right_bridge_in = new CvBridge<deprecated_msgs::Image>(&frame_msg.images[0], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
builtBridge = true;
}
Modified: pkg/trunk/vision/spacetime_stereo/sts_node.cpp
===================================================================
--- pkg/trunk/vision/spacetime_stereo/sts_node.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/spacetime_stereo/sts_node.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -65,8 +65,8 @@
#include <opencv/cxcore.h>
#include "ros/node.h"
#include <std_msgs/PointCloud.h>
-#include <std_msgs/ImageArray.h>
-#include <std_msgs/Image.h>
+#include <deprecated_msgs/ImageArray.h>
+#include <deprecated_msgs/Image.h>
#include "limits.h"
#include "image_utils/cv_bridge.h"
#include <vector>
@@ -221,11 +221,11 @@
{
public:
//IplImage *frame;
- std_msgs::ImageArray frame_msg;
+ deprecated_msgs::ImageArray frame_msg;
std_msgs::String cal_msg;
bool builtBridge;
- CvBridge<std_msgs::Image> *left_bridge_in;
- CvBridge<std_msgs::Image> *right_bridge_in;
+ CvBridge<deprecated_msgs::Image> *left_bridge_in;
+ CvBridge<deprecated_msgs::Image> *right_bridge_in;
//boost::mutex frame_mutex_;
bool IsCal;
@@ -279,8 +279,8 @@
//cvWaitKey(0);
if(!builtBridge) {
- left_bridge_in = new CvBridge<std_msgs::Image>(&frame_msg.images[1], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
- right_bridge_in = new CvBridge<std_msgs::Image>(&frame_msg.images[0], CvBridge<std_msgs::Image>::CORRECT_BGR | CvBridge<std_msgs::Image>::MAXDEPTH_8U);
+ left_bridge_in = new CvBridge<deprecated_msgs::Image>(&frame_msg.images[1], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
+ right_bridge_in = new CvBridge<deprecated_msgs::Image>(&frame_msg.images[0], CvBridge<deprecated_msgs::Image>::CORRECT_BGR | CvBridge<deprecated_msgs::Image>::MAXDEPTH_8U);
builtBridge = true;
}
Modified: pkg/trunk/vision/stereo_blob_tracker/include/blob_tracker.h
===================================================================
--- pkg/trunk/vision/stereo_blob_tracker/include/blob_tracker.h 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/stereo_blob_tracker/include/blob_tracker.h 2009-02-10 05:44:39 UTC (rev 10903)
@@ -53,7 +53,7 @@
// ROS
#include "ros/node.h"
-#include <std_msgs/Image.h>
+#include <deprecated_msgs/Image.h>
// OpenCV
#include <opencv/cv.h>
Modified: pkg/trunk/vision/stereo_blob_tracker/manifest.xml
===================================================================
--- pkg/trunk/vision/stereo_blob_tracker/manifest.xml 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/stereo_blob_tracker/manifest.xml 2009-02-10 05:44:39 UTC (rev 10903)
@@ -12,6 +12,7 @@
<url>http://www.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="deprecated_msgs"/>
<depend package="std_srvs"/>
<depend package="axis_cam"/>
<depend package="image_utils"/>
Modified: pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp
===================================================================
--- pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -2,7 +2,7 @@
#include <iostream>
#include "ros/node.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "std_msgs/PointStamped.h"
#include "axis_cam/PTZActuatorCmd.h"
#include "axis_cam/PTZActuatorState.h"
Modified: pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp
===================================================================
--- pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp 2009-02-10 05:43:30 UTC (rev 10902)
+++ pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp 2009-02-10 05:44:39 UTC (rev 10903)
@@ -2,7 +2,7 @@
#include <iostream>
#include "ros/node.h"
-#include "std_msgs/Image.h"
+#include "deprecated_msgs/Image.h"
#include "boost/thread/mutex.hpp"
@@ -15,7 +15,7 @@
#include <vector>
#include <map>
using namespace std;
-#include "std_msgs/ImageArray.h"
+#include "deprecated_msgs/ImageArray.h"
#include "std_msgs/String.h"
#include "std_msgs/PointStamped.h"
#include "stereo_blob_tracker/Rect2DStamped.h"
@@ -193,7 +193,7 @@
{
string label;
IplImage *cv_image;
- CvBridge<std_msgs::Image> *bridge;
+ CvBridge<deprecated_msgs::Image> *bridge;
};
// this class is originally copied from CvView in cv_view_array.cpp in vision/cv_view package.
@@ -208,7 +208,7 @@
// for subscrip...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-10 06:59:06
|
Revision: 10909
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10909&view=rev
Author: tfoote
Date: 2009-02-10 06:59:02 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
deprecating RobotBase2DOdom and Pose2DFloat32
Modified Paths:
--------------
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/segway_apox/manifest.xml
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_navfn.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_topological.cpp
pkg/trunk/motion_planning/mpglue/src/plan.cpp
pkg/trunk/motion_planning/mpglue/src/plan.h
pkg/trunk/motion_planning/mpglue/src/planner.h
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/amcl_player/cli.cpp
pkg/trunk/nav/amcl_player/manifest.xml
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/deadreckon/manifest.xml
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/nav/trajectory_rollout/manifest.xml
pkg/trunk/nav/trajectory_rollout/msg/ScoreMap2D.msg
pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/prcore/robot_msgs/manifest.xml
pkg/trunk/prcore/robot_msgs/msg/MapMetaData.msg
pkg/trunk/prcore/robot_msgs/msg/OccMap2D.msg
pkg/trunk/prcore/robot_msgs/msg/Planner2DGoal.msg
pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
pkg/trunk/prcore/robot_srvs/manifest.xml
pkg/trunk/simulators/rosstage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/logsetta/carmen/carmen_logger/carmen_logger.cpp
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
pkg/trunk/util/logsetta/odom/odom_extract.cpp
pkg/trunk/visualization/multitouch_nav/manifest.xml
pkg/trunk/visualization/multitouch_nav/skeleton.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/robot_base2d_pose_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/tools/pose_tool.cpp
pkg/trunk/world_models/topological_map/include/topological_map/ros_topological_planner.h
pkg/trunk/world_models/topological_map/manifest.xml
pkg/trunk/world_models/topological_map/srv/ConnectorCosts.srv
pkg/trunk/world_models/topological_map/srv/Nav2DPlan.srv
pkg/trunk/world_models/topological_map/srv/NavigationCost.srv
Added Paths:
-----------
pkg/trunk/deprecated/deprecated_msgs/msg/Pose2DFloat32.msg
pkg/trunk/deprecated/deprecated_msgs/msg/RobotBase2DOdom.msg
Modified: pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-02-10 06:59:02 UTC (rev 10909)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="tf" />
<depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
<depend package="roscpp" />
<depend package="pr2_mechanism_controllers" />
</package>
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-10 06:59:02 UTC (rev 10909)
@@ -42,7 +42,7 @@
// messages
-#include "std_msgs/RobotBase2DOdom.h"
+#include "deprecated_msgs/RobotBase2DOdom.h"
#include "std_msgs/PoseDot.h"
#include "std_msgs/PoseWithRatesStamped.h"
#include "robot_msgs/MechanismState.h"
@@ -79,7 +79,7 @@
void AngleOverflowCorrect(double& a, double ref);
// messages to receive
- std_msgs::RobotBase2DOdom _odom;
+ deprecated_msgs::RobotBase2DOdom _odom;
std_msgs::PoseWithRatesStamped _imu;
robot_msgs::MechanismState _mech;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-10 06:59:02 UTC (rev 10909)
@@ -53,7 +53,7 @@
#include <newmat10/newmatio.h>
#include <newmat10/newmatap.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <std_msgs/PoseDot.h>
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
@@ -185,9 +185,9 @@
void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
/*!
- * \brief Fill a structure of the form std_msgs::RobotBase2DOdom with odometry data
+ * \brief Fill a structure of the form deprecated_msgs::RobotBase2DOdom with odometry data
*/
- void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
+ void setOdomMessage(deprecated_msgs::RobotBase2DOdom &odom_msg_);
/*!
* \brief compute the desired wheel speeds
@@ -502,9 +502,9 @@
boost::mutex ros_lock_;
/*!
- * \brief std_msgs representation of an odometry message
+ * \brief deprecated_msgs representation of an odometry message
*/
- std_msgs::RobotBase2DOdom odom_msg_;
+ deprecated_msgs::RobotBase2DOdom odom_msg_;
double last_time_message_sent_ ;/** last time odometry message was published */
@@ -512,7 +512,7 @@
double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
- realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <deprecated_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
realtime_tools::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h 2009-02-10 06:59:02 UTC (rev 10909)
@@ -55,7 +55,7 @@
#include <newmat10/newmatio.h>
#include <newmat10/newmatap.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <std_msgs/PoseDot.h>
#include <pr2_msgs/Odometer.h>
#include <pr2_msgs/Covariance2D.h>
@@ -188,9 +188,9 @@
void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
/*!
- * \brief Fill a structure of the form std_msgs::RobotBase2DOdom with odometry data
+ * \brief Fill a structure of the form deprecated_msgs::RobotBase2DOdom with odometry data
*/
- void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
+ void setOdomMessage(deprecated_msgs::RobotBase2DOdom &odom_msg_);
/*!
* \brief compute the desired wheel speeds
@@ -491,7 +491,7 @@
/*!
* \brief std_msgs representation of an odometry message
*/
- std_msgs::RobotBase2DOdom odom_msg_;
+ deprecated_msgs::RobotBase2DOdom odom_msg_;
double last_time_message_sent_ ;/** last time odometry message was published */
@@ -499,7 +499,7 @@
double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
- realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
+ realtime_tools::RealtimePublisher <deprecated_msgs::RobotBase2DOdom>* publisher_ ; //!< Publishes the odometry msg from the update() realtime loop
realtime_tools::RealtimePublisher <tf::tfMessage>* transform_publisher_ ; //!< Publishes the odom to base transform msg from the update() realtime loop
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-02-10 06:59:02 UTC (rev 10909)
@@ -14,6 +14,7 @@
<depend package="rospy" />
<depend package="libTF" />
<depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
<depend package="pr2_msgs" />
<depend package="robot_msgs" />
<depend package="robot_kinematics" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -571,7 +571,7 @@
base_casters_[i].controller_.update();
}
-void BaseController::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
+void BaseController::setOdomMessage(deprecated_msgs::RobotBase2DOdom &odom_msg_)
{
odom_msg_.header.frame_id = "odom";
odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
@@ -1081,7 +1081,7 @@
if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <deprecated_msgs::RobotBase2DOdom> ("odom", 1) ;
if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete odometer_publisher_ ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -680,7 +680,7 @@
base_casters_[i].position_controller_.update();
}
-void BaseControllerPos::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
+void BaseControllerPos::setOdomMessage(deprecated_msgs::RobotBase2DOdom &odom_msg_)
{
odom_msg_.header.frame_id = "odom";
odom_msg_.header.stamp.sec = (unsigned long)floor(robot_state_->hw_->current_time_);
@@ -1148,7 +1148,7 @@
if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
- publisher_ = new realtime_tools::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+ publisher_ = new realtime_tools::RealtimePublisher <deprecated_msgs::RobotBase2DOdom> ("odom", 1) ;
if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete odometer_publisher_ ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -35,7 +35,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
#include <robot_srvs/SetJointCmd.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
static int done = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -35,7 +35,7 @@
#include <libTF/libTF.h>
#include <ros/node.h>
#include <std_msgs/PoseDot.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <std_msgs/Quaternion.h>
#include <iostream>
#include <fstream>
@@ -87,7 +87,7 @@
~test_run_base() {}
- std_msgs::RobotBase2DOdom odom;
+ deprecated_msgs::RobotBase2DOdom odom;
void odomMsgReceived()
{
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -36,7 +36,7 @@
#include <ros/node.h>
#include <std_msgs/PoseWithRatesStamped.h>
#include <std_msgs/PoseDot.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <std_msgs/Quaternion.h>
static int done = 0;
@@ -88,7 +88,7 @@
std_msgs::PoseWithRatesStamped ground_truth;
- std_msgs::RobotBase2DOdom odom;
+ deprecated_msgs::RobotBase2DOdom odom;
int subscriber_connected;
Added: pkg/trunk/deprecated/deprecated_msgs/msg/Pose2DFloat32.msg
===================================================================
--- pkg/trunk/deprecated/deprecated_msgs/msg/Pose2DFloat32.msg (rev 0)
+++ pkg/trunk/deprecated/deprecated_msgs/msg/Pose2DFloat32.msg 2009-02-10 06:59:02 UTC (rev 10909)
@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 th
Added: pkg/trunk/deprecated/deprecated_msgs/msg/RobotBase2DOdom.msg
===================================================================
--- pkg/trunk/deprecated/deprecated_msgs/msg/RobotBase2DOdom.msg (rev 0)
+++ pkg/trunk/deprecated/deprecated_msgs/msg/RobotBase2DOdom.msg 2009-02-10 06:59:02 UTC (rev 10909)
@@ -0,0 +1,4 @@
+Header header
+Pose2DFloat32 pos
+Pose2DFloat32 vel
+byte stall
Modified: pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
===================================================================
--- pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-10 06:59:02 UTC (rev 10909)
@@ -47,7 +47,7 @@
#include <ros/time.h>
#include <urdf/URDF.h>
#include <planning_models/kinematic.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <tf/transform_listener.h>
#include <cmath>
@@ -301,7 +301,7 @@
tf::TransformListener m_tf;
ros::Node *m_node;
- std_msgs::RobotBase2DOdom m_localizedPose;
+ deprecated_msgs::RobotBase2DOdom m_localizedPose;
bool m_haveBasePos;
robot_msgs::MechanismState m_mechanismState;
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2009-02-10 06:59:02 UTC (rev 10909)
@@ -86,7 +86,7 @@
//rosTF
#include "tf/transform_broadcaster.h"
// Messages that I need
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
//#include <std_msgs/RobotBase2DCmdVel.h>
#include <std_msgs/PoseDot.h>
#include <robot_msgs/BatteryState.h>
@@ -102,7 +102,7 @@
public:
QueuePointer q;
- std_msgs::RobotBase2DOdom odom;
+ deprecated_msgs::RobotBase2DOdom odom;
std_msgs::PoseDot cmdvel;
tf::TransformBroadcaster tf;
@@ -118,7 +118,7 @@
// TODO: remove XDR dependency
playerxdr_ftable_init();
- advertise<std_msgs::RobotBase2DOdom>("odom", 1);
+ advertise<deprecated_msgs::RobotBase2DOdom>("odom", 1);
advertise<robot_msgs::BatteryState>("battery_state", 1);
// The Player address that will be assigned to this device. The format
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-02-10 06:59:02 UTC (rev 10909)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
+ <depend package="deprecated_msgs" />
<depend package="robot_msgs" />
<depend package="phase_space" />
<depend package="tf" />
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -46,7 +46,7 @@
#include "robot_msgs/MocapBody.h"
#include "std_msgs/Transform.h"
-#include "std_msgs/RobotBase2DOdom.h"
+#include "deprecated_msgs/RobotBase2DOdom.h"
#include "std_msgs/PoseWithRatesStamped.h"
#include <tf/tf.h>
@@ -74,7 +74,7 @@
param("~publish_transform", publish_transform_, true) ;
param("~base_id", base_id_, 1) ;
- advertise<std_msgs::RobotBase2DOdom>("localizedpose", 1);
+ advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose", 1);
advertise<std_msgs::PoseWithRatesStamped>("base_pose_ground_truth", 1) ;
m_tfServer = new tf::TransformBroadcaster(*this);
@@ -151,7 +151,7 @@
private:
robot_msgs::MocapSnapshot snapshot_;
- std_msgs::RobotBase2DOdom m_currentPos;
+ deprecated_msgs::RobotBase2DOdom m_currentPos;
tf::TransformBroadcaster *m_tfServer;
unsigned int publish_success_count_ ;
unsigned int publish_attempt_count_ ;
Modified: pkg/trunk/drivers/robot/segway_apox/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/segway_apox/manifest.xml 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/drivers/robot/segway_apox/manifest.xml 2009-02-10 06:59:02 UTC (rev 10909)
@@ -3,6 +3,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<depend package="roscpp"/>
<depend package="tf"/>
</package>
Modified: pkg/trunk/drivers/robot/segway_apox/segway.cpp
===================================================================
--- pkg/trunk/drivers/robot/segway_apox/segway.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/drivers/robot/segway_apox/segway.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -4,7 +4,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
#include "std_msgs/PoseDot.h"
-#include "std_msgs/RobotBase2DOdom.h"
+#include "deprecated_msgs/RobotBase2DOdom.h"
#include "std_msgs/String.h"
#include "rmp_frame.h"
extern "C" {
@@ -34,7 +34,7 @@
static const int max_x_stepsize = 5, max_yaw_stepsize = 2;
std_msgs::PoseDot cmd_vel;
- std_msgs::RobotBase2DOdom odom;
+ deprecated_msgs::RobotBase2DOdom odom;
std_msgs::String op_mode;
rmp_frame_t rmp;
dgc_usbcan_p can;
@@ -68,7 +68,7 @@
req_timeout(false)
{
odom.header.frame_id = "odom";
- advertise<std_msgs::RobotBase2DOdom>("odom", 1);
+ advertise<deprecated_msgs::RobotBase2DOdom>("odom", 1);
subscribe("cmd_vel", cmd_vel, &Segway::cmd_vel_cb, 1);
subscribe("operating_mode", op_mode, &Segway::op_mode_cb, 1);
}
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h 2009-02-10 06:59:02 UTC (rev 10909)
@@ -43,7 +43,7 @@
#include "odom_estimation.h"
// messages
-#include "std_msgs/RobotBase2DOdom.h"
+#include "deprecated_msgs/RobotBase2DOdom.h"
#include "std_msgs/PoseDot.h"
#include "std_msgs/PoseWithRatesStamped.h"
#include "std_msgs/PoseStamped.h"
@@ -91,7 +91,7 @@
// messages to receive
std_msgs::PoseDot vel_;
- std_msgs::RobotBase2DOdom odom_;
+ deprecated_msgs::RobotBase2DOdom odom_;
std_msgs::PoseWithRatesStamped imu_;
robot_msgs::VOPose vo_;
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-10 06:59:02 UTC (rev 10909)
@@ -56,7 +56,7 @@
#include <robot_msgs/Planner2DGoal.h>
#include <std_msgs/PoseDot.h>
#include <laser_scan/LaserScan.h>
-#include <std_msgs/RobotBase2DOdom.h>
+#include <deprecated_msgs/RobotBase2DOdom.h>
#include <std_msgs/PointCloud.h>
// For transform support
@@ -124,7 +124,7 @@
* @brief Overwrites the current plan with a new one. Will handle suitable publication
* @see publishPlan
*/
- void updatePlan(const std::list<std_msgs::Pose2DFloat32>& newPlan);
+ void updatePlan(const std::list<deprecated_msgs::Pose2DFloat32>& newPlan);
/**
* @brief Overwrites the current plan with a new one. Will handle suitable publication
@@ -199,7 +199,7 @@
*/
void stopRobot();
- void publishPath(bool isGlobal, const std::list<std_msgs::Pose2DFloat32>& path);
+ void publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path);
void publishFootprint(double x, double y, double th);
@@ -230,7 +230,7 @@
std_msgs::PointCloud tiltCloudMsg_; /**< Filled by subscriber with new tilte laser scans */
std_msgs::PointCloud groundPlaneCloudMsg_; /**< Filled by subscriber with point clouds */
std_msgs::PointCloud stereoCloudMsg_; /**< Filled by subscriber with point clouds */
- std_msgs::RobotBase2DOdom odomMsg_; /**< Odometry in the odom frame picked up by subscription */
+ deprecated_msgs::RobotBase2DOdom odomMsg_; /**< Odometry in the odom frame picked up by subscription */
laser_scan::LaserProjection projector_; /**< Used to project laser scans */
tf::TransformListener tf_; /**< Used to do transforms */
@@ -258,13 +258,13 @@
tf::Stamped<tf::Pose> global_pose_; /**< The global pose in the map frame */
- std_msgs::RobotBase2DOdom base_odom_; /**< Odometry in the base frame */
+ deprecated_msgs::RobotBase2DOdom base_odom_; /**< Odometry in the base frame */
/** Parameters that will be passed on initialization soon */
double baseLaserMaxRange_; /**< Used in laser scan projection */
double tiltLaserMaxRange_; /**< Used in laser scan projection */
- std::list<std_msgs::Pose2DFloat32> plan_; /**< The 2D plan in grid co-ordinates of the cost map */
+ std::list<deprecated_msgs::Pose2DFloat32> plan_; /**< The 2D plan in grid co-ordinates of the cost map */
// Filter parameters
double minZ_;
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-10 06:59:02 UTC (rev 10909)
@@ -15,6 +15,7 @@
<depend package="roscpp"/>
<depend package="rosconsole"/>
<depend package="std_msgs"/>
+ <depend package="deprecated_msgs"/>
<depend package="robot_msgs"/>
<depend package="robot_srvs"/>
<!--depend package="std_srvs"/ Trying to remove Tully-->
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeGoal.msg 2009-02-10 06:59:02 UTC (rev 10909)
@@ -6,7 +6,7 @@
float32 recharge_level
# Sets the pose for accessing the plug
-std_msgs/Pose2DFloat32 pose
+deprecated_msgs/Pose2DFloat32 pose
# True to submit a goal, false to recall it
byte enable
Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg 2009-02-10 06:59:02 UTC (rev 10909)
@@ -22,4 +22,4 @@
float32 goal_recharge_level
# The goal pose for accessing the plug
-std_msgs/Pose2DFloat32 goal_pose
+deprecated_msgs/Pose2DFloat32 goal_pose
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-10 06:57:18 UTC (rev 10908)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-10 06:59:02 UTC (rev 10909)
@@ -36,7 +36,7 @@
#include <highlevel_controllers/move_base.hh>
#include <std_msgs/PoseDot.h>
#include <std_msgs/PointCloud.h>
-#include <std_msgs/Pose2DFloat32.h>
+#include <deprecated_msgs/Pose2DFloat32.h>
#include <std_msgs/Polyline2D.h>
#include <robot_srvs/StaticMap.h>
#include <std_msgs/PointStamped.h>
@@ -514,7 +514,7 @@
stopRobot();
}
- void MoveBase::updatePlan(const std::list<std_msgs::Pose2DFloat32>& newPlan){
+ void MoveBase::updatePlan(const std::list<deprecated_msgs::Pose2DFloat32>& newPlan){
lock();
// If we have a valid plan then only swap in the new plan if it is shorter.
@@ -527,7 +527,7 @@
unlock();
}
- /** \todo Some code duplication wrt MoveBase::updatePlan(const std::list<std_msgs::Pose2DFloat32>&). */
+ /** \todo Some code duplication wrt MoveBase::updatePlan(const std::list<deprecated_msgs::Pose2DFloat32>&). */
void MoveBase::updatePlan(mpglue::waypoint_plan_t const & newPlan) {
sentry<MoveBase> guard(this);
if (!isValid() || plan_.size() > newPlan.size()){
@@ -542,8 +542,8 @@
* applied to protect access to the plan.
*/
bool MoveBase::inCollision() const {
- for(std::list<std_msgs::Pose2DFloat32>::const_iterator it = plan_.begin(); it != plan_.end(); ++it){
- const std_msgs::Pose2DFloat32& w = *it;
+ for(std::list<deprecated_msgs::Pose2DFloat32>::const_iterator it = plan_.begin(); it != plan_.end(); ++it){
+ const deprecated_msgs::Pose2DFloat32& w = *it;
unsigned int ind = costMap_->WC_IND(w.x, w.y);
if((*costMap_)[ind] >= CostMap2D::INSCRIBED_INFLATED_OBSTACLE){
ROS_DEBUG("path in collision at <%f, %f>\n", w.x, w.y);
@@ -569,13 +569,13 @@
publish("robot_footprint", footprint_msg);
}
- void MoveBase::publishPath(bool isGlobal, const std::list<std_msgs::Pose2DFloat32>& path) {
+ void MoveBase::publishPath(bool isGlobal, const std::list<deprecated_msgs::Pose2DFloat32>& path) {
std_msgs::Polyline2D guiPathMsg;
guiPathMsg.set_points_size(path.size());
unsigned int i = 0;
- for(std::list<std_msgs::Pose2DFloat32>::const_iterator it = path.begin(); it != path.end(); ++it){
- const std_msgs::Pose2DFloat32& w = *it;
+ for(std::list<deprecated_msgs::Pose2DFloat32>::const_iterator it = path.begin(); it != path.end(); ++it)...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-10 07:34:27
|
Revision: 10913
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10913&view=rev
Author: tfoote
Date: 2009-02-10 07:34:24 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
removing references to std_msgs/Position and copying across PointCloud and PointStamped
Modified Paths:
--------------
pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
pkg/trunk/vision/visual_odometry/src/marker.py
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/ChannelFloat32.msg
pkg/trunk/prcore/robot_msgs/msg/PointCloud.msg
pkg/trunk/prcore/robot_msgs/msg/PointStamped.msg
Added: pkg/trunk/prcore/robot_msgs/msg/ChannelFloat32.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/ChannelFloat32.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/ChannelFloat32.msg 2009-02-10 07:34:24 UTC (rev 10913)
@@ -0,0 +1,2 @@
+string name
+float32[] vals
\ No newline at end of file
Added: pkg/trunk/prcore/robot_msgs/msg/PointCloud.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/PointCloud.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/PointCloud.msg 2009-02-10 07:34:24 UTC (rev 10913)
@@ -0,0 +1,3 @@
+Header header
+Point32[] pts
+ChannelFloat32[] chan
Added: pkg/trunk/prcore/robot_msgs/msg/PointStamped.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/PointStamped.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/PointStamped.msg 2009-02-10 07:34:24 UTC (rev 10913)
@@ -0,0 +1,2 @@
+Header header
+Point point
Modified: pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg 2009-02-10 07:29:12 UTC (rev 10912)
+++ pkg/trunk/prcore/robot_msgs/msg/VisualizationMarker.msg 2009-02-10 07:34:24 UTC (rev 10913)
@@ -32,4 +32,4 @@
int32 g # green color value
int32 b # blue color value
#These are only used if the object type is LINE_STRIP
-std_msgs/Position[] points
+Point[] points
Modified: pkg/trunk/vision/visual_odometry/src/marker.py
===================================================================
--- pkg/trunk/vision/visual_odometry/src/marker.py 2009-02-10 07:29:12 UTC (rev 10912)
+++ pkg/trunk/vision/visual_odometry/src/marker.py 2009-02-10 07:34:24 UTC (rev 10913)
@@ -34,7 +34,7 @@
import roslib
roslib.load_manifest('visual_odometry')
-from std_msgs.msg import Position
+from std_msgs.msg import Point
from robot_msgs.msg import VisualizationMarker
import rospy
from visualodometer import Pose
@@ -123,7 +123,7 @@
marker.points = points
vm_pub.publish(marker)
- marker.points = [Position(p.x, p.y, -tableh) for p in points]
+ marker.points = [Point(p.x, p.y, -tableh) for p in points]
marker.id += 200
marker.r = 0
marker.g = 0
@@ -155,7 +155,7 @@
x /= factor
y /= factor
z /= factor
- return Position(x,y,z)
+ return Point(x,y,z)
for i,(x,y) in enumerate([ (0,0), (640,0), (0,480), (640,480)]):
model = [(0,0,0), cam.pix2cam(x, y, 640)]
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-10 08:29:40
|
Revision: 10914
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10914&view=rev
Author: tfoote
Date: 2009-02-10 08:29:30 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
fixing ogre_visualizer build
Modified Paths:
--------------
pkg/trunk/prcore/robot_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
Modified: pkg/trunk/prcore/robot_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicSpaceParameters.msg 2009-02-10 07:34:24 UTC (rev 10913)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicSpaceParameters.msg 2009-02-10 08:29:30 UTC (rev 10914)
@@ -17,8 +17,8 @@
# Lower coordinate for a box defining the volume in the workspace the
# motion planner is active in. If planning in 2D, only first 2 values
# (x, y) of the points are used.
-std_msgs/Position volumeMin
+std_msgs/Point volumeMin
# Higher coordinate for the box described above
-std_msgs/Position volumeMax
+std_msgs/Point volumeMax
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 07:34:24 UTC (rev 10913)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/marker_display.cpp 2009-02-10 08:29:30 UTC (rev 10914)
@@ -300,11 +300,11 @@
line->clear();
line->setLineWidth( message->xScale );
- std::vector<std_msgs::Position>::iterator it = message->points.begin();
- std::vector<std_msgs::Position>::iterator end = message->points.end();
+ std::vector<robot_msgs::Point>::iterator it = message->points.begin();
+ std::vector<robot_msgs::Point>::iterator end = message->points.end();
for ( ; it != end; ++it )
{
- std_msgs::Position& p = *it;
+ robot_msgs::Point& p = *it;
Ogre::Vector3 v( p.x, p.y, p.z );
robotToOgre( v );
Modified: pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp 2009-02-10 07:34:24 UTC (rev 10913)
+++ pkg/trunk/visualization/ogre_visualizer/src/test/marker_test.cpp 2009-02-10 08:29:30 UTC (rev 10914)
@@ -60,7 +60,7 @@
line_marker.b = 0;
for ( int i = -50; i < 50; ++i )
{
- std_msgs::Position p;
+ robot_msgs::Point p;
p.x = 1;
p.y = (i*2);
p.z = 0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-10 15:20:26
|
Revision: 10918
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10918&view=rev
Author: hsujohnhsu
Date: 2009-02-10 15:20:20 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
update manifest for messages location change.
Modified Paths:
--------------
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-02-10 11:45:15 UTC (rev 10917)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-02-10 15:20:20 UTC (rev 10918)
@@ -22,4 +22,5 @@
<depend package="transformations"/>
<depend package="ransac_ground_plane_extraction"/>
<depend package="point_cloud_assembler"/>
+ <depend package="deprecated_msgs"/>
</package>
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-02-10 11:45:15 UTC (rev 10917)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-02-10 15:20:20 UTC (rev 10918)
@@ -11,4 +11,5 @@
<depend package="pr2_prototype1_gazebo" />
<depend package="pr2_gazebo" />
<depend package="gazebo_robot_description" />
+ <depend package="deprecated_msgs" /> <!-- RobotBase2DOdom -->
</package>
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-02-10 11:45:15 UTC (rev 10917)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/manifest.xml 2009-02-10 15:20:20 UTC (rev 10918)
@@ -11,5 +11,7 @@
<depend package="pr2_prototype1_gazebo" />
<depend package="pr2_gazebo" />
<depend package="gazebo_robot_description" />
+ <depend package="laser_scan" />
+ <depend package="image_msgs" />
<sysdepend os="ubuntu" version="7.04-feisty" package="xvfb"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-10 19:48:19
|
Revision: 10929
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10929&view=rev
Author: hsujohnhsu
Date: 2009-02-10 19:48:16 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
typo fix in arm_defs.
gripper_defs dummy comment change.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-10 19:39:33 UTC (rev 10928)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-10 19:48:16 UTC (rev 10929)
@@ -69,21 +69,22 @@
TEST_TIMEOUT = 50.0
# pre-recorded poses for above commands
-TARGET_ARM_TX = 0.697079458962
-TARGET_ARM_TY = 0.298321990173
-TARGET_ARM_TZ = 0.788284959682
-TARGET_ARM_QX = 0.237303684388
-TARGET_ARM_QY = -0.183131139797
-TARGET_ARM_QZ = 0.190090503477
-TARGET_ARM_QW = 0.934887986606
-TARGET_GRIPPER_TX = 0.759604836385
-TARGET_GRIPPER_TY = 0.327703622909
-TARGET_GRIPPER_TZ = 0.825872564018
-TARGET_GRIPPER_QX = 0.201779841484
-TARGET_GRIPPER_QY = -0.221575348589
-TARGET_GRIPPER_QZ = 0.349992333295
-TARGET_GRIPPER_QW = 0.88752162064
+TARGET_PALM_TX = 0.707932405635
+TARGET_PALM_TY = 0.302486634619
+TARGET_PALM_TZ = 0.784961973515
+TARGET_PALM_QX = 0.237333965585
+TARGET_PALM_QY = -0.182954011269
+TARGET_PALM_QZ = 0.190066678152
+TARGET_PALM_QW = 0.934919823513
+TARGET_FNGR_TX = 0.770409128528
+TARGET_FNGR_TY = 0.332355930864
+TARGET_FNGR_TZ = 0.822766287069
+TARGET_FNGR_QX = 0.201867284399
+TARGET_FNGR_QY = -0.221524091779
+TARGET_FNGR_QZ = 0.349958602096
+TARGET_FNGR_QW = 0.887527832279
+
class ArmTest(unittest.TestCase):
def __init__(self, *args):
super(ArmTest, self).__init__(*args)
@@ -113,15 +114,15 @@
def fngrP3dInput(self, p3d):
i = 0
- pos_error = abs(p3d.pos.position.x - TARGET_GRIPPER_TX) + \
- abs(p3d.pos.position.y - TARGET_GRIPPER_TY) + \
- abs(p3d.pos.position.z - TARGET_GRIPPER_TZ)
+ pos_error = abs(p3d.pos.position.x - TARGET_FNGR_TX) + \
+ abs(p3d.pos.position.y - TARGET_FNGR_TY) + \
+ abs(p3d.pos.position.z - TARGET_FNGR_TZ)
#target pose rotation matrix
- target_rm = rotation_matrix_from_quaternion([TARGET_GRIPPER_QX \
- ,TARGET_GRIPPER_QY \
- ,TARGET_GRIPPER_QZ \
- ,TARGET_GRIPPER_QW])
+ target_rm = rotation_matrix_from_quaternion([TARGET_FNGR_QX \
+ ,TARGET_FNGR_QY \
+ ,TARGET_FNGR_QZ \
+ ,TARGET_FNGR_QW])
#p3d pose quaternion
p3d_q = [p3d.pos.orientation.x \
@@ -157,15 +158,15 @@
def palmP3dInput(self, p3d):
i = 0
- pos_error = abs(p3d.pos.position.x - TARGET_ARM_TX) + \
- abs(p3d.pos.position.y - TARGET_ARM_TY) + \
- abs(p3d.pos.position.z - TARGET_ARM_TZ)
+ pos_error = abs(p3d.pos.position.x - TARGET_PALM_TX) + \
+ abs(p3d.pos.position.y - TARGET_PALM_TY) + \
+ abs(p3d.pos.position.z - TARGET_PALM_TZ)
#target pose rotation matrix
- target_rm = rotation_matrix_from_quaternion([TARGET_ARM_QX \
- ,TARGET_ARM_QY \
- ,TARGET_ARM_QZ \
- ,TARGET_ARM_QW])
+ target_rm = rotation_matrix_from_quaternion([TARGET_PALM_QX \
+ ,TARGET_PALM_QY \
+ ,TARGET_PALM_QZ \
+ ,TARGET_PALM_QW])
#p3d pose quaternion
p3d_q = [p3d.pos.orientation.x \
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-02-10 19:39:33 UTC (rev 10928)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-02-10 19:48:16 UTC (rev 10929)
@@ -288,7 +288,7 @@
<inertial>
<mass value="1.90327" />
- <com xyz="0.01014.00032 -0.01211" />
+ <com xyz="0.01014 0.00032 -0.01211" />
<inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614"
iyy="0.00441606455" iyz="-0.00003968914"
izz="0.00359156824" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-02-10 19:39:33 UTC (rev 10928)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-02-10 19:48:16 UTC (rev 10929)
@@ -471,7 +471,7 @@
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_dummy">
+ <geometry name="${side}_gripper_collision_dummy">
<box size="0.1 0.1 0.1" />
</geometry>
</collision>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-10 23:09:37
|
Revision: 10946
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10946&view=rev
Author: hsujohnhsu
Date: 2009-02-10 21:55:05 +0000 (Tue, 10 Feb 2009)
Log Message:
-----------
rospy.subscribe_topic() --> rospy.Subscriber()
Modified Paths:
--------------
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.py
pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.py
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -161,10 +161,10 @@
print "LNK\n"
#pub_base = rospy.Publisher("cmd_vel", BaseVel)
pub_goal = rospy.Publisher("goal", Planner2DGoal) #received by wavefront_player or equivalent
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom" , RobotBase2DOdom , self.odomInput)
- rospy.subscribe_topic("base_bumper" , String , self.bumpedInput)
- rospy.subscribe_topic("torso_lift_bumper" , String , self.bumpedInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom" , RobotBase2DOdom , self.odomInput)
+ rospy.Subscriber("base_bumper" , String , self.bumpedInput)
+ rospy.Subscriber("torso_lift_bumper" , String , self.bumpedInput)
rospy.init_node(NAME, anonymous=True)
Modified: pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.py
===================================================================
--- pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_gazebo_plugin/test_pendulum.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -140,8 +140,8 @@
def test_pendulum(self):
print "LNK\n"
- rospy.subscribe_topic("link1_pose", PoseWithRatesStamped, self.p3dInput1)
- rospy.subscribe_topic("link2_pose", PoseWithRatesStamped, self.p3dInput2)
+ rospy.Subscriber("link1_pose", PoseWithRatesStamped, self.p3dInput1)
+ rospy.Subscriber("link2_pose", PoseWithRatesStamped, self.p3dInput2)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 20.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_collision_gazebo/test_slide.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -90,7 +90,7 @@
def test_slide(self):
print "LINK\n"
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.positionInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.positionInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 50.0
while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -205,8 +205,8 @@
print "LNK\n"
pub_arm = rospy.Publisher("left_arm_commands", JointPosCmd)
pub_gripper = rospy.Publisher("l_gripper_controller/set_command", Float64)
- rospy.subscribe_topic("l_gripper_palm_pose_ground_truth", PoseWithRatesStamped, self.palmP3dInput)
- rospy.subscribe_topic("l_gripper_l_finger_pose_ground_truth", PoseWithRatesStamped, self.fngrP3dInput)
+ rospy.Subscriber("l_gripper_palm_pose_ground_truth", PoseWithRatesStamped, self.palmP3dInput)
+ rospy.Subscriber("l_gripper_l_finger_pose_ground_truth", PoseWithRatesStamped, self.fngrP3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_TIMEOUT
while not rospy.is_shutdown() and (not self.palm_success or not self.fngr_success) and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -212,8 +212,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -173,8 +173,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -173,8 +173,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -212,8 +212,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -173,8 +173,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 10.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -118,8 +118,8 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", PoseDot)
- rospy.subscribe_topic("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.subscribe_topic("odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
+ rospy.Subscriber("odom", RobotBase2DOdom, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 60.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -178,8 +178,8 @@
print " wait TEST_INIT_WAIT sec for objects to settle and arms to tuck "
time.sleep(TEST_INIT_WAIT)
print " subscribe stereo left image from ROS "
- #rospy.subscribe_topic("test_camera/image", Image, self.imageInput) # this is a test camera, simply looking at the cups
- rospy.subscribe_topic("stereo_l/image", Image, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
+ #rospy.Subscriber("test_camera/image", Image, self.imageInput) # this is a test camera, simply looking at the cups
+ rospy.Subscriber("stereo_l/image", Image, self.imageInput) # this is a camera mounted on PR2 head (left stereo camera)
rospy.init_node(NAME, anonymous=True)
#self.pollThread.start()
timeout_t = time.time() + TEST_DURATION
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.py 2009-02-10 21:53:18 UTC (rev 10945)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_scan.py 2009-02-10 21:55:05 UTC (rev 10946)
@@ -287,7 +287,7 @@
def test_scan(self):
print "LNK\n"
- rospy.subscribe_topic("base_scan", LaserScan, self.pointInput)
+ rospy.Subscriber("base_scan", LaserScan, self.pointInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-11 00:21:04
|
Revision: 10961
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10961&view=rev
Author: tfoote
Date: 2009-02-11 00:20:50 +0000 (Wed, 11 Feb 2009)
Log Message:
-----------
this is a major change. It is changing many messages from std_msgs to robot_msgs. The buildtest passes, I willneed to vet the tests afterwords ros#447 There will be a log migration tool coming out soon to deal with these changes, but this will break logs with tf_messages in it right now.
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position_ground_truth.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_orientation_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_pose_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_torque_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_trajectory_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_velocity_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_orientation_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_pose_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_trajectory_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/demos/2dnav_pr2/parse_logs.py
pkg/trunk/demos/2dnav_pr2/pose_listen.py
pkg/trunk/deprecated/deprecated_srvs/manifest.xml
pkg/trunk/deprecated/deprecated_srvs/srv/PolledStereoCloud.srv
pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/blob_finder.h
pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/scan_assembler.h
pkg/trunk/deprecated/point_cloud_utils/include/point_cloud_utils/timed_scan_assembler.h
pkg/trunk/deprecated/point_cloud_utils/manifest.xml
pkg/trunk/deprecated/point_cloud_utils/src/blob_finder.cpp
pkg/trunk/deprecated/point_cloud_utils/src/scan_assembler.cpp
pkg/trunk/deprecated/point_cloud_utils/src/timed_scan_assembler.cpp
pkg/trunk/deprecated/ransac_ground_plane_extraction/include/ransac_ground_plane_extraction/ransac_ground_plane_extraction.h
pkg/trunk/deprecated/ransac_ground_plane_extraction/include/ransac_ground_plane_extraction/ransac_ground_plane_extraction_node.h
pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction.cpp
pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp
pkg/trunk/deprecated/robot_filter/include/robot_filter/RobotFilter.h
pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
pkg/trunk/deprecated/scan_utils/include/dataTypes.h
pkg/trunk/deprecated/scan_utils/include/listen_node/scanListenNode.h
pkg/trunk/deprecated/scan_utils/include/smartScan.h
pkg/trunk/deprecated/scan_utils/manifest.xml
pkg/trunk/deprecated/scan_utils/msg/OctreeMsg.msg
pkg/trunk/deprecated/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/deprecated/scan_utils/src/dataTypes.cpp
pkg/trunk/deprecated/scan_utils/src/listen_node/scanListenNode.cpp
pkg/trunk/deprecated/scan_utils/src/smartScan.cpp
pkg/trunk/drivers/cam/dcam/src/nodes/dcam.cpp
pkg/trunk/drivers/imu/imu_node/imu_node.cc
pkg/trunk/drivers/laser/laser_scan_annotator/manifest.xml
pkg/trunk/drivers/laser/laser_scan_annotator/msg/LaserScanAnnotated.msg
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/katana/srv/KatanaIK.srv
pkg/trunk/drivers/robot/katana/srv/KatanaPose.srv
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/laser_scan_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler_srv.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/test/test_assembler.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_block_laser.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_f3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_stereo_camera.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_block_laser.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_f3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_stereo_camera.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
pkg/trunk/estimation/robot_pose_ekf/test/test_ekf.cpp
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/BaseStateAdapter.hh
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp
pkg/trunk/manip/spacenav_node/manifest.xml
pkg/trunk/manip/spacenav_node/src/spacenav_node.cpp
pkg/trunk/manip/teleop_spacenav/manifest.xml
pkg/trunk/manip/teleop_spacenav/spacenav_teleop.py
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/mapping/door_handle_detector/manifest.xml
pkg/trunk/mapping/door_handle_detector/scripts/find_door.py
pkg/trunk/mapping/door_handle_detector/src/door_handle_detector.cpp
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.cpp
pkg/trunk/mapping/door_handle_detector/src/geometric_helper.h
pkg/trunk/mapping/door_handle_detector/test/door_handle_detection_test.cpp
pkg/trunk/mapping/door_handle_detector/test/open_door_executive_test.cpp
pkg/trunk/mapping/planar_patch_map/manifest.xml
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map.cpp
pkg/trunk/mapping/planar_patch_map/src/planar_patch_map_omp.cpp
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/angles.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/areas.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/distances.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/intersections.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/nearest.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/point.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/projections.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/statistics.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/transforms.h
pkg/trunk/mapping/point_cloud_mapping/include/cloud_kdtree/kdtree.h
pkg/trunk/mapping/point_cloud_mapping/include/point_cloud_mapping/cloud_io.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_circle.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_cylinder.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_line.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_oriented_line.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_plane.h
pkg/trunk/mapping/point_cloud_mapping/include/sample_consensus/sac_model_sphere.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_downsampler.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/distances.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/intersections.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/nearest.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/point.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/statistics.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/transforms.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/misc.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/read.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/bag_to_pcd.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/cloudmsg_to_screen.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/tools/pcd_to_msg.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_io/write.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/convert.cpp
pkg/trunk/mapping/point_cloud_mapping/src/cloud_kdtree/search.cpp
pkg/trunk/mapping/point_cloud_mapping/src/normal_estimation.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/mlesac.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_circle.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_cylinder.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_line.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_oriented_line.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_plane.cpp
pkg/trunk/mapping/point_cloud_mapping/src/sample_consensus/sac_model_sphere.cpp
pkg/trunk/mapping/point_cloud_mapping/test/cloud_io/test_io.cpp
pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/bunny_model.h
pkg/trunk/mapping/point_cloud_mapping/test/cloud_kdtree/test_kdtree.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_circle_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_cylinder_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_line_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_plane_fit.cpp
pkg/trunk/mapping/point_cloud_mapping/test/sample_consensus/test_sphere_fit.cpp
pkg/trunk/mapping/semantic_point_annotator/manifest.xml
pkg/trunk/mapping/semantic_point_annotator/src/sac_ground_removal.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator.cpp
pkg/trunk/mapping/semantic_point_annotator/src/semantic_point_annotator_omp.cpp
pkg/trunk/mapping/stereo_convex_patch_histogram/manifest.xml
pkg/trunk/mapping/stereo_convex_patch_histogram/src/convex_patch_histogram.cpp
pkg/trunk/mapping/table_object_detector/src/table_object_detector.cpp
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/include/robarm3d/plan_path_node.h
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/sim_grasp_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/src/test/test_plan_path.cpp
pkg/trunk/motion_planning/planning_research/robarm3d/srv/PlanPathSrv.srv
pkg/trunk/nav/deadreckon/deadreckon.cpp
pkg/trunk/nav/deadreckon/manifest.xml
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view_sdl/nav_view.cpp
pkg/trunk/nav/teleop_base/teleop_base.cpp
pkg/trunk/nav/teleop_base_keyboard/manifest.xml
pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/point_grid.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/trajectory_rollout/src/point_grid.cpp
pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/pr2_msgs/msg/GraspPoint.msg
pkg/trunk/pr2_msgs/msg/Plane.msg
pkg/trunk/pr2_msgs/msg/PlaneStamped.msg
pkg/trunk/pr2_srvs/manifest.xml
pkg/trunk/pr2_srvs/srv/TransientObstacles.srv
pkg/trunk/prcore/pytf/manifest.xml
pkg/trunk/prcore/robot_msgs/msg/Door.msg
pkg/trunk/prcore/robot_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/prcore/robot_msgs/msg/MocapBody.msg
pkg/trunk/prcore/robot_msgs/msg/MocapMarker.msg
pkg/trunk/prcore/robot_msgs/msg/ObjectOnTable.msg
pkg/trunk/prcore/robot_msgs/msg/OrientedBoundingBox.msg
pkg/trunk/prcore/robot_msgs/msg/ParticleCloud.msg
pkg/trunk/prcore/robot_msgs/msg/PoseWithCovariance.msg
pkg/trunk/prcore/robot_msgs/msg/PositionMeasurement.msg
pkg/trunk/prcore/robot_msgs/msg/Twist.msg
pkg/trunk/prcore/robot_msgs/msg/VOPose.msg
pkg/trunk/prcore/robot_msgs/msg/Wrench.msg
pkg/trunk/prcore/robot_srvs/srv/GetQuaternion.srv
pkg/trunk/prcore/robot_srvs/srv/GetVector.srv
pkg/trunk/prcore/robot_srvs/srv/IKService.srv
pkg/trunk/prcore/robot_srvs/srv/SetVector.srv
pkg/trunk/prcore/tf/include/tf/transform_broadcaster.h
pkg/trunk/prcore/tf/include/tf/transform_datatypes.h
pkg/trunk/prcore/tf/include/tf/transform_listener.h
pkg/trunk/prcore/tf/manifest.xml
pkg/trunk/prcore/tf/msg/tfMessage.msg
pkg/trunk/prcore/tf/src/tf.cpp
pkg/trunk/prcore/tf/src/transform_listener.cpp
pkg/trunk/prcore/tf/test/test_notifier.cpp
pkg/trunk/prcore/tf/test/tf_unittest.cpp
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
pkg/trunk/util/laser_scan/include/laser_scan/laser_scan.h
pkg/trunk/util/laser_scan/src/laser_scan.cc
pkg/trunk/util/laser_scan/src/scan_shadows_filter.cpp
pkg/trunk/util/logsetta/imu/imu_extract.cpp
pkg/trunk/util/message_sequencing/CMakeLists.txt
pkg/trunk/util/message_sequencing/example/testdelay.cpp
pkg/trunk/util/message_sequencing/example/testgen.cpp
pkg/trunk/util/message_sequencing/example/testrecv.cpp
pkg/trunk/util/message_sequencing/manifest.xml
pkg/trunk/util/robot_self_filter/manifest.xml
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/vision/calib_converter/manifest.xml
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/checkerboard_detector/manifest.xml
pkg/trunk/vision/checkerboard_detector/msg/Object6DPose.msg
pkg/trunk/vision/laser_processor/laser_processor.cpp
pkg/trunk/vision/laser_processor/laser_processor.h
pkg/trunk/vision/people/src/filter/detector_particle.cpp
pkg/trunk/vision/people/src/filter/detector_particle.h
pkg/trunk/vision/people/src/filter/mcpdf_pos_vel.cpp
pkg/trunk/vision/people/src/filter/mcpdf_pos_vel.h
pkg/trunk/vision/people/src/filter/mcpdf_vector.cpp
pkg/trunk/vision/people/src/filter/mcpdf_vector.h
pkg/trunk/vision/people/src/filter/people_tracking_node.cpp
pkg/trunk/vision/people/src/filter/people_tracking_node.h
pkg/trunk/vision/people/src/filter/tracker_kalman.cpp
pkg/trunk/vision/people/src/filter/tracker_particle.cpp
pkg/trunk/vision/people/src/filter/tracker_particle.h
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/people_follower.cpp
pkg/trunk/vision/people/src/people_follower.h
pkg/trunk/vision/people/src/py.cpp
pkg/trunk/vision/people/src/stereo_face_feature_tracker.py
pkg/trunk/vision/spacetime_stereo/manifest.xml
pkg/trunk/vision/spacetime_stereo/sts_dynamic.cpp
pkg/trunk/vision/spacetime_stereo/sts_node.cpp
pkg/trunk/vision/stereo_blob_tracker/src/axis_blob_tracker.cpp
pkg/trunk/vision/stereo_blob_tracker/src/listener.cpp
pkg/trunk/vision/stereo_blob_tracker/src/stereo_blob_tracker.cpp
pkg/trunk/vision/stereo_capture/manifest.xml
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
pkg/trunk/vision/visual_odometry/src/marker.py
pkg/trunk/visualization/cloud_viewer/manifest.xml
pkg/trunk/visualization/cloud_viewer/src/cloud_node/cloud_node.cpp
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/collision_map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/laser_scan_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_base.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_base.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/point_cloud_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/poly_line_2d_display.h
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/polygonal_map_display.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/polygonal_map_display.h
pkg/trunk/visualization/ogre_visualizer/src/test/cloud_test.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap2d_pqueue_benchmark.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/test/benchmark.cc
pkg/trunk/world_models/costmap_2d/src/test/costmap2d_pqueue_benchmark.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
pkg/trunk/world_models/static_map_server/manifest.xml
pkg/trunk/world_models/static_map_server/nodes/map_server
pkg/trunk/world_models/topological_map/manifest.xml
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/msg/Polygon3D.msg
pkg/trunk/prcore/robot_msgs/msg/PolygonalMap.msg
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-11 00:20:50 UTC (rev 10961)
@@ -13,7 +13,7 @@
BOOST_LIBRARY_DIRS=$(shell rosboost-cfg --lib_dirs)
installed: $(SVN_DIR) patched
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
+ cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include:$(CPATH)" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && mkdir -p build && cd build && BOOST_INCLUDEDIR=$(BOOST_INCLUDE_DIRS) BOOST_LIBRARYDIR=$(BOOST_LIBRARY_DIRS) cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release -DBoost_INCLUDE_DIR=$(BOOST_INCLUDE_DIRS) -DBoost_LIBRARY_DIR=$(BOOST_LIBRARY_DIRS) .. && export PARALLEL_JOBS=ROS_PARALLEL_JOBS && make $(ROS_PARALLEL_JOBS) install
touch installed
clean:
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-02-11 00:20:50 UTC (rev 10961)
@@ -11,7 +11,7 @@
robot_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
-std_msgs/PointCloud laser_cloud
+robot_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
robot_msgs/MocapSnapshot mocap_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-02-11 00:20:50 UTC (rev 10961)
@@ -16,7 +16,7 @@
robot_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
-std_msgs/PointCloud laser_cloud
+robot_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
robot_msgs/MocapSnapshot phase_space_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/joy_cmd_head.py 2009-02-11 00:20:50 UTC (rev 10961)
@@ -3,7 +3,7 @@
import roslib; roslib.load_manifest('kinematic_calibration')
import sys
import rospy
-from std_msgs.msg import PointStamped, Point
+from robot_msgs.msg import PointStamped, Point
from time import sleep
from joy.msg import Joy
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-02-11 00:20:50 UTC (rev 10961)
@@ -40,7 +40,7 @@
#include "robot_msgs/MocapSnapshot.h"
#include "std_msgs/Empty.h"
-#include "std_msgs/PointCloud.h"
+#include "robot_msgs/PointCloud.h"
#include "image_msgs/RawStereo.h"
#include "kinematic_calibration/CalibrationData.h"
@@ -85,8 +85,8 @@
boost::mutex raw_stereo_lock_ ;
// Point Cloud Messages
- std_msgs::PointCloud laser_cloud_ ;
- std_msgs::PointCloud safe_laser_cloud_ ;
+ robot_msgs::PointCloud laser_cloud_ ;
+ robot_msgs::PointCloud safe_laser_cloud_ ;
boost::mutex laser_cloud_lock_ ;
unsigned int capture_count_ ;
Modified: pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/laser_camera_calibration/octave/startgathering.m 2009-02-11 00:20:50 UTC (rev 10961)
@@ -15,6 +15,7 @@
rosoct_add_msgs('std_msgs');
rosoct_add_msgs('checkerboard_detector');
rosoct_add_msgs('robot_msgs');
+rosoct_add_msgs('laser_scan');
queuesize = 1000; % need big buffer
g_calibdata = {};
@@ -28,7 +29,7 @@
lastlaserscan = {};
rosoct_unsubscribe("new_tile_scan");
-success = rosoct_subscribe("new_tilt_scan", @std_msgs_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
+success = rosoct_subscribe("new_tilt_scan", @laser_scan_LaserScan, @(msg) laserscancb(msg,robot),queuesize);
if( ~success )
error('subscribe failed');
end
Modified: pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-02-11 00:20:50 UTC (rev 10961)
@@ -4,7 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="tf" />
- <depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="deprecated_msgs" />
<depend package="roscpp" />
<depend package="pr2_mechanism_controllers" />
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-02-11 00:20:50 UTC (rev 10961)
@@ -53,7 +53,7 @@
_completed(false)
{
// advertise the velocity commands
- advertise<std_msgs::PoseDot>("cmd_vel",10);
+ advertise<robot_msgs::PoseDot>("cmd_vel",10);
// subscribe to messages
subscribe("odom", _odom, &odom_calib::odom_callback, 10);
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-02-11 00:20:50 UTC (rev 10961)
@@ -43,8 +43,8 @@
// messages
#include "deprecated_msgs/RobotBase2DOdom.h"
-#include "std_msgs/PoseDot.h"
-#include "std_msgs/PoseWithRatesStamped.h"
+#include "robot_msgs/PoseDot.h"
+#include "robot_msgs/PoseWithRatesStamped.h"
#include "robot_msgs/MechanismState.h"
namespace calibration
@@ -80,11 +80,11 @@
// messages to receive
deprecated_msgs::RobotBase2DOdom _odom;
- std_msgs::PoseWithRatesStamped _imu;
+ robot_msgs::PoseWithRatesStamped _imu;
robot_msgs::MechanismState _mech;
// estimated robot pose message to send
- std_msgs::PoseDot _vel;
+ robot_msgs::PoseDot _vel;
// service messages
pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-10 23:54:34 UTC (rev 10960)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h 2009-02-11 00:20:50 UTC (rev 10961)
@@ -54,7 +54,7 @@
#include <newmat10/newmatap.h>
#include <depre...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-11 03:22:16
|
Revision: 10969
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10969&view=rev
Author: tfoote
Date: 2009-02-11 02:35:23 +0000 (Wed, 11 Feb 2009)
Log Message:
-----------
moving Point2DFloat32 and Position2DInt into personalrobots
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/areas.h
pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
pkg/trunk/motion_planning/mpglue/src/footprint.cpp
pkg/trunk/motion_planning/mpglue/src/footprint.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/costmap_model.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/point_grid.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/world_model.h
pkg/trunk/nav/trajectory_rollout/msg/GlobalPlan.msg
pkg/trunk/nav/trajectory_rollout/src/costmap_model.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/trajectory_rollout/src/point_grid.cpp
pkg/trunk/nav/trajectory_rollout/src/trajectory_controller.cpp
pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
pkg/trunk/nav/trajectory_rollout/test/utest.cpp
pkg/trunk/pr2_msgs/manifest.xml
pkg/trunk/pr2_msgs/msg/OccDiff.msg
pkg/trunk/pr2_srvs/manifest.xml
pkg/trunk/prcore/robot_msgs/msg/Polyline2D.msg
Added Paths:
-----------
pkg/trunk/deprecated/deprecated_msgs/msg/Point2DFloat32.msg
pkg/trunk/nav/trajectory_rollout/msg/Position2DInt.msg
Added: pkg/trunk/deprecated/deprecated_msgs/msg/Point2DFloat32.msg
===================================================================
--- pkg/trunk/deprecated/deprecated_msgs/msg/Point2DFloat32.msg (rev 0)
+++ pkg/trunk/deprecated/deprecated_msgs/msg/Point2DFloat32.msg 2009-02-11 02:35:23 UTC (rev 10969)
@@ -0,0 +1,2 @@
+float32 x
+float32 y
\ No newline at end of file
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-02-11 02:35:23 UTC (rev 10969)
@@ -87,7 +87,7 @@
public:
- typedef std::vector<std_msgs::Point2DFloat32> footprint_t;
+ typedef std::vector<deprecated_msgs::Point2DFloat32> footprint_t;
/**
* @brief Constructor
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -240,7 +240,7 @@
global_map_accessor_ = new CostMapAccessor(*costMap_);
local_map_accessor_ = new CostMapAccessor(*costMap_, mapSize, 0.0, 0.0);
- std_msgs::Point2DFloat32 pt;
+ deprecated_msgs::Point2DFloat32 pt;
//create a square footprint
pt.x = robotRadius;
pt.y = -1 * robotRadius;
@@ -555,7 +555,7 @@
}
void MoveBase::publishFootprint(double x, double y, double th){
- std::vector<std_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
+ std::vector<deprecated_msgs::Point2DFloat32> footprint = controller_->drawFootprint(x, y, th);
robot_msgs::Polyline2D footprint_msg;
footprint_msg.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
Modified: pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/areas.h
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/areas.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/mapping/point_cloud_mapping/include/cloud_geometry/areas.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -36,7 +36,7 @@
// ROS includes
#include <robot_msgs/Point32.h>
#include <robot_msgs/PointCloud.h>
-#include <std_msgs/Point2DFloat32.h>
+#include <deprecated_msgs/Point2DFloat32.h>
#include <robot_msgs/Polygon3D.h>
#include <robot_msgs/Polyline2D.h>
@@ -54,7 +54,7 @@
* \param p2 the second Point2DFloat32 point
*/
inline bool
- comparePoint2DFloat32 (const std_msgs::Point2DFloat32& p1, const std_msgs::Point2DFloat32& p2)
+ comparePoint2DFloat32 (const deprecated_msgs::Point2DFloat32& p1, const deprecated_msgs::Point2DFloat32& p2)
{
if (p1.x < p2.x) return (true);
else if (p1.x > p2.x) return (false);
@@ -81,7 +81,7 @@
double compute2DPolygonalArea (robot_msgs::PointCloud points, std::vector<double> normal);
double compute2DPolygonalArea (robot_msgs::Polygon3D polygon, std::vector<double> normal);
void convexHull2D (robot_msgs::PointCloud *points, std::vector<int> *indices, std::vector<double> *coeff, robot_msgs::Polygon3D &hull);
- void convexHull2D (std::vector<std_msgs::Point2DFloat32> points, robot_msgs::Polyline2D &hull);
+ void convexHull2D (std::vector<deprecated_msgs::Point2DFloat32> points, robot_msgs::Polyline2D &hull);
bool isPointIn2DPolygon (robot_msgs::Point32 point, robot_msgs::Polygon3D *polygon);
}
Modified: pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp
===================================================================
--- pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/mapping/point_cloud_mapping/src/cloud_geometry/areas.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -155,7 +155,7 @@
centroid (1) /= epoints.size ();
// Push projected centered 2d points
- std::vector<std_msgs::Point2DFloat32> epoints_demean (epoints.size ());
+ std::vector<deprecated_msgs::Point2DFloat32> epoints_demean (epoints.size ());
for (unsigned int cp = 0; cp < indices->size (); cp++)
{
epoints_demean[cp].x = epoints[cp](k1) - centroid (0);
@@ -213,7 +213,7 @@
* \param hull the resultant 2D convex hull model as a \a Polyline2D
*/
void
- convexHull2D (std::vector<std_msgs::Point2DFloat32> points, robot_msgs::Polyline2D &hull)
+ convexHull2D (std::vector<deprecated_msgs::Point2DFloat32> points, robot_msgs::Polyline2D &hull)
{
int nr_points = points.size ();
hull.points.resize (nr_points + 1);
Modified: pkg/trunk/motion_planning/mpglue/src/footprint.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/footprint.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/motion_planning/mpglue/src/footprint.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -49,7 +49,7 @@
void initSimpleFootprint(footprint_t & footprint,
double inscribedRadius, double circumscribedRadius)
{
- std_msgs::Point2DFloat32 pt;
+ deprecated_msgs::Point2DFloat32 pt;
//create a square footprint
pt.x = inscribedRadius;
Modified: pkg/trunk/motion_planning/mpglue/src/footprint.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/footprint.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/motion_planning/mpglue/src/footprint.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -35,12 +35,12 @@
#ifndef MPGLUE_FOOTPRINT_HPP
#define MPGLUE_FOOTPRINT_HPP
-#include <std_msgs/Point2DFloat32.h>
+#include <deprecated_msgs/Point2DFloat32.h>
#include <vector>
namespace mpglue {
- typedef std::vector<std_msgs::Point2DFloat32> footprint_t;
+ typedef std::vector<deprecated_msgs::Point2DFloat32> footprint_t;
footprint_t createSimpleFootprint(double inscribedRadius, double circumscribedRadius);
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/costmap_model.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/costmap_model.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/costmap_model.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -69,7 +69,7 @@
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return True if all points lie outside the footprint, false otherwise
*/
- virtual bool legalFootprint(const std_msgs::Point2DFloat32& position, const std::vector<std_msgs::Point2DFloat32>& footprint,
+ virtual bool legalFootprint(const deprecated_msgs::Point2DFloat32& position, const std::vector<deprecated_msgs::Point2DFloat32>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
@@ -77,7 +77,7 @@
* @param observations The observations from various sensors
* @param laser_outline The polygon of the active sensor region
*/
- virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<std_msgs::Point2DFloat32>& laser_outline){}
+ virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<deprecated_msgs::Point2DFloat32>& laser_outline){}
private:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -47,7 +47,7 @@
//for GUI debugging
#include <robot_msgs/Polyline2D.h>
-#include <std_msgs/Point2DFloat32.h>
+#include <deprecated_msgs/Point2DFloat32.h>
//for transform support
@@ -127,7 +127,7 @@
class GovernorNode: public ros::Node
{
public:
- GovernorNode(std::vector<std_msgs::Point2DFloat32> footprint_spec);
+ GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec);
//callback for when the planned passes a new map
void planReceived();
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/point_grid.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/point_grid.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/point_grid.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -39,7 +39,7 @@
#include <vector>
#include <list>
#include <cfloat>
-#include <std_msgs/Point2DFloat32.h>
+#include <deprecated_msgs/Point2DFloat32.h>
#include <robot_msgs/Point32.h>
#include <costmap_2d/observation.h>
#include <trajectory_rollout/world_model.h>
@@ -65,7 +65,7 @@
* @param obstacle_range The maximum distance for obstacles to be added to the grid
* @param min_separation The minimum distance between points in the grid
*/
- PointGrid(double width, double height, double resolution, std_msgs::Point2DFloat32 origin,
+ PointGrid(double width, double height, double resolution, deprecated_msgs::Point2DFloat32 origin,
double max_z, double obstacle_range, double min_separation);
/**
@@ -79,7 +79,7 @@
* @param upper_right The upper right corner of the range search
* @param points A vector of pointers to lists of the relevant points
*/
- void getPointsInRange(std_msgs::Point2DFloat32 lower_left, std_msgs::Point2DFloat32 upper_right, std::vector< std::list<robot_msgs::Point32>* > points);
+ void getPointsInRange(deprecated_msgs::Point2DFloat32 lower_left, deprecated_msgs::Point2DFloat32 upper_right, std::vector< std::list<robot_msgs::Point32>* > points);
/**
* @brief Checks if any points in the grid lie inside a convex footprint
@@ -89,7 +89,7 @@
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return True if all points lie outside the footprint, false otherwise
*/
- virtual bool legalFootprint(const std_msgs::Point2DFloat32& position, const std::vector<std_msgs::Point2DFloat32>& footprint,
+ virtual bool legalFootprint(const deprecated_msgs::Point2DFloat32& position, const std::vector<deprecated_msgs::Point2DFloat32>& footprint,
double inscribed_radius, double circumscribed_radius);
/**
@@ -97,7 +97,7 @@
* @param observations The observations from various sensors
* @param laser_outline The polygon of the active sensor region
*/
- virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<std_msgs::Point2DFloat32>& laser_outline);
+ virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<deprecated_msgs::Point2DFloat32>& laser_outline);
/**
* @brief Convert from world coordinates to grid coordinates
@@ -106,7 +106,7 @@
* @param gy The y coordinate of the corresponding grid cell to be set by the function
* @return True if the conversion was successful, false otherwise
*/
- inline bool gridCoords(std_msgs::Point2DFloat32 pt, unsigned int& gx, unsigned int& gy) const {
+ inline bool gridCoords(deprecated_msgs::Point2DFloat32 pt, unsigned int& gx, unsigned int& gy) const {
if(pt.x < origin_.x || pt.y < origin_.y){
gx = 0;
gy = 0;
@@ -131,7 +131,7 @@
* @param lower_left The lower left bounds of the cell in world coordinates to be filled in
* @param upper_right The upper right bounds of the cell in world coordinates to be filled in
*/
- inline void getCellBounds(unsigned int gx, unsigned int gy, std_msgs::Point2DFloat32& lower_left, std_msgs::Point2DFloat32& upper_right) const {
+ inline void getCellBounds(unsigned int gx, unsigned int gy, deprecated_msgs::Point2DFloat32& lower_left, deprecated_msgs::Point2DFloat32& upper_right) const {
lower_left.x = gx * resolution_ + origin_.x;
lower_left.y = gy * resolution_ + origin_.y;
@@ -201,7 +201,7 @@
* @param c The point to compute orientation for
* @return orient(a, b, c) < 0 ----> Left, orient(a, b, c) > 0 ----> Right
*/
- inline double orient(const std_msgs::Point2DFloat32& a, const std_msgs::Point2DFloat32& b, const robot_msgs::Point32& c){
+ inline double orient(const deprecated_msgs::Point2DFloat32& a, const deprecated_msgs::Point2DFloat32& b, const robot_msgs::Point32& c){
double acx = a.x - c.x;
double bcx = b.x - c.x;
double acy = a.y - c.y;
@@ -215,7 +215,7 @@
* @param poly The polygon to check against
* @return True if the point is in the polygon, false otherwise
*/
- bool ptInPolygon(const robot_msgs::Point32& pt, const std::vector<std_msgs::Point2DFloat32>& poly);
+ bool ptInPolygon(const robot_msgs::Point32& pt, const std::vector<deprecated_msgs::Point2DFloat32>& poly);
/**
* @brief Insert a point into the point grid
@@ -243,11 +243,11 @@
* @brief Removes points from the grid that lie within the polygon
* @param poly A specification of the polygon to clear from the grid
*/
- void removePointsInPolygon(const std::vector<std_msgs::Point2DFloat32> poly);
+ void removePointsInPolygon(const std::vector<deprecated_msgs::Point2DFloat32> poly);
private:
double resolution_; ///< @brief The resolution of the grid in meters/cell
- std_msgs::Point2DFloat32 origin_; ///< @brief The origin point of the grid
+ deprecated_msgs::Point2DFloat32 origin_; ///< @brief The origin point of the grid
unsigned int width_; ///< @brief The width of the grid in cells
unsigned int height_; ///< @brief The height of the grid in cells
std::vector< std::list<robot_msgs::Point32> > cells_; ///< @brief Storage for the cells in the grid
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -52,8 +52,8 @@
#include <trajectory_rollout/trajectory.h>
//we'll take in a path as a vector of points
-#include <std_msgs/Point2DFloat32.h>
-#include <std_msgs/Position2DInt.h>
+#include <deprecated_msgs/Point2DFloat32.h>
+#include <trajectory_rollout/Position2DInt.h>
//for computing path distance
#include <queue>
@@ -97,7 +97,7 @@
*/
TrajectoryController(WorldModel& world_model,
const costmap_2d::ObstacleMapAccessor& ma,
- std::vector<std_msgs::Point2DFloat32> footprint_spec,
+ std::vector<deprecated_msgs::Point2DFloat32> footprint_spec,
double inscribed_radius, double circumscribed_radius,
double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0,
double sim_time = 1.0, double sim_granularity = 0.025, int samples_per_dim = 20,
@@ -129,7 +129,7 @@
* @brief Update the plan that the controller is following
* @param new_plan A new plan for the controller to follow
*/
- void updatePlan(const std::vector<std_msgs::Point2DFloat32>& new_plan);
+ void updatePlan(const std::vector<deprecated_msgs::Point2DFloat32>& new_plan);
/**
* @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
@@ -138,7 +138,7 @@
* @param theta_i The orientation of the robot
* @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
*/
- std::vector<std_msgs::Point2DFloat32> drawFootprint(double x_i, double y_i, double theta_i);
+ std::vector<deprecated_msgs::Point2DFloat32> drawFootprint(double x_i, double y_i, double theta_i);
/**
* @brief Accessor for the goal the robot is currently pursuing in world corrdinates
@@ -214,7 +214,7 @@
* @param fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
* @return The cells that make up either the outline or entire footprint of the robot depending on fill
*/
- std::vector<std_msgs::Position2DInt> getFootprintCells(double x_i, double y_i, double theta_i, bool fill);
+ std::vector<trajectory_rollout::Position2DInt> getFootprintCells(double x_i, double y_i, double theta_i, bool fill);
/**
* @brief Use Bresenham's algorithm to trace a line between two points in a grid
@@ -224,13 +224,13 @@
* @param y1 The y coordinate of the second point
* @param pts Will be filled with the cells that lie on the line in the grid
*/
- void getLineCells(int x0, int x1, int y0, int y1, std::vector<std_msgs::Position2DInt>& pts);
+ void getLineCells(int x0, int x1, int y0, int y1, std::vector<trajectory_rollout::Position2DInt>& pts);
/**
* @brief Fill the outline of a polygon, in this case the robot footprint, in a grid
* @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
*/
- void getFillCells(std::vector<std_msgs::Position2DInt>& footprint);
+ void getFillCells(std::vector<trajectory_rollout::Position2DInt>& footprint);
/**
* @brief Update what cells are considered path based on the global plan
@@ -241,11 +241,11 @@
const costmap_2d::ObstacleMapAccessor& ma_; ///< @brief Provides access to cost map information
WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
- std::vector<std_msgs::Point2DFloat32> footprint_spec_; ///< @brief The footprint specification of the robot
+ std::vector<deprecated_msgs::Point2DFloat32> footprint_spec_; ///< @brief The footprint specification of the robot
double inscribed_radius_, circumscribed_radius_; ///< @brief The inscribed and circumscribed radii of the robot
- std::vector<std_msgs::Point2DFloat32> global_plan_; ///< @brief The global path for the robot to follow
+ std::vector<deprecated_msgs::Point2DFloat32> global_plan_; ///< @brief The global path for the robot to follow
bool stuck_left, stuck_right; ///< @brief Booleans to keep the robot from oscillating during rotation
bool rotating_left, rotating_right; ///< @brief Booleans to keep track of the direction of rotation for the robot
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -47,8 +47,7 @@
#include <tf/transform_datatypes.h>
-#include <std_msgs/Point2DFloat32.h>
-#include <std_msgs/Position2DInt.h>
+#include <deprecated_msgs/Point2DFloat32.h>
#include <robot_msgs/PoseDot.h>
namespace trajectory_rollout {
@@ -67,7 +66,7 @@
* @param circumscribed_radius The circumscribed radius of the robot
*/
TrajectoryControllerROS(ros::Node& ros_node,
- const costmap_2d::CostMapAccessor& ma, std::vector<std_msgs::Point2DFloat32> footprint_spec,
+ const costmap_2d::CostMapAccessor& ma, std::vector<deprecated_msgs::Point2DFloat32> footprint_spec,
double inscribed_radius, double cirumscribed_radius);
/**
@@ -82,7 +81,7 @@
* @param theta_i The orientation of the robot
* @return A vector of points in world coordinates that correspond to the verticies of the robot's footprint
*/
- std::vector<std_msgs::Point2DFloat32> drawFootprint(double x_i, double y_i, double theta_i);
+ std::vector<deprecated_msgs::Point2DFloat32> drawFootprint(double x_i, double y_i, double theta_i);
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/world_model.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/world_model.h 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/world_model.h 2009-02-11 02:35:23 UTC (rev 10969)
@@ -39,7 +39,7 @@
#include <vector>
#include <costmap_2d/observation.h>
-#include <std_msgs/Point2DFloat32.h>
+#include <deprecated_msgs/Point2DFloat32.h>
namespace trajectory_rollout {
/**
@@ -54,7 +54,7 @@
* @param observations The observations from various sensors
* @param laser_outline The polygon of the active sensor region
*/
- virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<std_msgs::Point2DFloat32>& laser_outline) = 0;
+ virtual void updateWorld(const std::vector<costmap_2d::Observation>& observations, const std::vector<deprecated_msgs::Point2DFloat32>& laser_outline) = 0;
/**
* @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
@@ -64,7 +64,7 @@
* @param circumscribed_radius The radius of the circumscribed circle of the robot
* @return True if the footprint is legal based on the world model, false otherwise
*/
- virtual bool legalFootprint(const std_msgs::Point2DFloat32& position, const std::vector<std_msgs::Point2DFloat32>& footprint,
+ virtual bool legalFootprint(const deprecated_msgs::Point2DFloat32& position, const std::vector<deprecated_msgs::Point2DFloat32>& footprint,
double inscribed_radius, double circumscribed_radius) = 0;
/**
Modified: pkg/trunk/nav/trajectory_rollout/msg/GlobalPlan.msg
===================================================================
--- pkg/trunk/nav/trajectory_rollout/msg/GlobalPlan.msg 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/msg/GlobalPlan.msg 2009-02-11 02:35:23 UTC (rev 10969)
@@ -1 +1 @@
-std_msgs/Point2DFloat32[] path
+deprecated_msgs/Point2DFloat32[] path
Added: pkg/trunk/nav/trajectory_rollout/msg/Position2DInt.msg
===================================================================
--- pkg/trunk/nav/trajectory_rollout/msg/Position2DInt.msg (rev 0)
+++ pkg/trunk/nav/trajectory_rollout/msg/Position2DInt.msg 2009-02-11 02:35:23 UTC (rev 10969)
@@ -0,0 +1,2 @@
+int64 x
+int64 y
\ No newline at end of file
Modified: pkg/trunk/nav/trajectory_rollout/src/costmap_model.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/costmap_model.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/src/costmap_model.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -37,7 +37,7 @@
#include <trajectory_rollout/costmap_model.h>
using namespace std;
-using namespace std_msgs;
+using namespace deprecated_msgs;
using namespace costmap_2d;
namespace trajectory_rollout {
Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -38,7 +38,7 @@
using namespace trajectory_rollout;
namespace trajectory_rollout{
- GovernorNode::GovernorNode(std::vector<std_msgs::Point2DFloat32> footprint_spec) :
+ GovernorNode::GovernorNode(std::vector<deprecated_msgs::Point2DFloat32> footprint_spec) :
ros::Node("governor_node"), map_(MAP_SIZE_X, MAP_SIZE_Y),
tf_(*this, true, (uint64_t)10000000000ULL),
ma_(map_, OUTER_RADIUS),
@@ -86,7 +86,7 @@
ma_.synchronize();
//update the global plan from the message
- vector<std_msgs::Point2DFloat32> plan;
+ vector<deprecated_msgs::Point2DFloat32> plan;
for(unsigned int i = 0; i < plan_msg_.plan.get_path_size(); ++i){
plan.push_back(plan_msg_.plan.path[i]);
@@ -176,7 +176,7 @@
publish("local_path", path_msg);
printf("path msg\n");
- vector<std_msgs::Point2DFloat32> footprint = tc_.drawFootprint(x, y, th);
+ 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.set_points_size(footprint.size());
footprint_msg.color.r = 1.0;
@@ -223,8 +223,8 @@
int main(int argc, char** argv){
ros::init(argc, argv);
- std::vector<std_msgs::Point2DFloat32> footprint_spec;
- std_msgs::Point2DFloat32 pt;
+ std::vector<deprecated_msgs::Point2DFloat32> footprint_spec;
+ deprecated_msgs::Point2DFloat32 pt;
//create a square footprint
pt.x = ROBOT_FRONT_RADIUS;
pt.y = ROBOT_SIDE_RADIUS;
Modified: pkg/trunk/nav/trajectory_rollout/src/point_grid.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/point_grid.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/src/point_grid.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -42,12 +42,12 @@
using namespace std;
using namespace robot_msgs;
-using namespace std_msgs;
+using namespace deprecated_msgs;
using namespace costmap_2d;
namespace trajectory_rollout {
-PointGrid::PointGrid(double size_x, double size_y, double resolution, std_msgs::Point2DFloat32 origin, double max_z, double obstacle_range, double min_seperation) :
+PointGrid::PointGrid(double size_x, double size_y, double resolution, deprecated_msgs::Point2DFloat32 origin, double max_z, double obstacle_range, double min_seperation) :
resolution_(resolution), origin_(origin), max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range), sq_min_separation_(min_seperation * min_seperation)
{
width_ = (int) (size_x / resolution_);
Modified: pkg/trunk/nav/trajectory_rollout/src/trajectory_controller.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/trajectory_controller.cpp 2009-02-11 02:31:59 UTC (rev 10968)
+++ pkg/trunk/nav/trajectory_rollout/src/trajectory_controller.cpp 2009-02-11 02:35:23 UTC (rev 10969)
@@ -38,13 +38,13 @@
#include <trajectory_rollout/trajectory_controller.h>
using namespace std;
-using namespace std_msgs;
+using namespace deprecated_msgs;
using namespace costmap_2d;
namespace trajectory_rollout{
TrajectoryController::TrajectoryController(WorldModel& world_model,
const costmap_2d::ObstacleMapAccessor& ma,
- std::vector<std_msgs::Point2DFloat32> footprint_spec,
+ std::vector<deprecated_msgs::Point2DFloat32> footprint_spec,
double inscribed_radius, double circumscribed_radius,
double acc_lim_x, double acc_lim_y, double acc_lim_theta,
double sim_time, double sim_granularity, int samples_per_dim,
@@ -625,8 +625,8 @@
Trajectory TrajectoryController::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
tf::Stamped<tf::Pose>& drive_velocities, vector<costmap_2d::Observation> obse...
[truncated message content] |
|
From: <rph...@us...> - 2009-02-12 00:27:27
|
Revision: 11017
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11017&view=rev
Author: rphilippsen
Date: 2009-02-12 00:27:17 +0000 (Thu, 12 Feb 2009)
Log Message:
-----------
hooked 16-connected 2D grids into mpglue, mpbench, and highlevel_controllers (#925)
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
pkg/trunk/motion_planning/mpbench/mkhtml.sh
pkg/trunk/motion_planning/mpbench/src/setup.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_sbpl.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -162,7 +162,9 @@
boost::shared_ptr<mpglue::IndexTransform> mit(mpglue::createIndexTransform(&getCostMap()));
if ("2D" == environmentType)
- env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit));
+ env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit, false));
+ if ("2D16" == environmentType)
+ env_.reset(mpglue::SBPLEnvironment::create2D(mcm, mit, true));
else if ("3DKIN" == environmentType) {
string const prefix("env3d/");
//// ignored by SBPL (at least in r9900).
@@ -179,7 +181,7 @@
timetoturn45degsinplace_secs, 0));
}
else {
- ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D or 3DKIN",
+ ROS_ERROR("in MoveBaseSBPL ctor: invalid environmentType \"%s\", use 2D, 2D16 or 3DKIN",
environmentType.c_str());
throw int(2);
}
Modified: pkg/trunk/motion_planning/mpbench/mkhtml.sh
===================================================================
--- pkg/trunk/motion_planning/mpbench/mkhtml.sh 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpbench/mkhtml.sh 2009-02-12 00:27:17 UTC (rev 11017)
@@ -2,7 +2,7 @@
MPBENCH=`rospack find mpbench`/mpbench-base
-CONSTANT_OPTS="-w hc:cubicle:1.2:3"
+CONSTANT_OPTS="-w xml:../data/scrape-corner.xml"
CONSTANT_HR=""
A_OPT="-p"
Modified: pkg/trunk/motion_planning/mpbench/src/setup.cpp
===================================================================
--- pkg/trunk/motion_planning/mpbench/src/setup.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpbench/src/setup.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -911,10 +911,18 @@
shared_ptr<mpglue::SBPLEnvironment> sbpl_environment;
if ("2d" == envstr) {
if (progress_os)
- *progress_os << " creating 2DEnvironment\n" << flush;
+ *progress_os << " creating 8-connected 2DEnvironment\n" << flush;
sbpl_environment.reset(mpglue::SBPLEnvironment::create2D(setup->getCostmap(),
- setup->getIndexTransform()));
+ setup->getIndexTransform(),
+ false));
}
+ else if ("2d16" == envstr) {
+ if (progress_os)
+ *progress_os << " creating 16-connected 2DEnvironment\n" << flush;
+ sbpl_environment.reset(mpglue::SBPLEnvironment::create2D(setup->getCostmap(),
+ setup->getIndexTransform(),
+ true));
+ }
else if ("3dkin" == envstr) {
if (progress_os)
*progress_os << " creating 3DKINEnvironment\n" << flush;
@@ -930,7 +938,7 @@
}
else
throw runtime_error("mpbench::Setup::create(): invalid environment token \""
- + envstr + "\", must be \"2d\" or \"3dkin\"");
+ + envstr + "\", must be \"2d\", \"2d16\" or \"3dkin\"");
if ( ! sbpl_environment)
throw runtime_error("mpbench::Setup::create(): failed to create SBPLEnvironment from \""
+ envstr + "\"");
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -87,7 +87,8 @@
{
public:
SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it);
+ boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV2D * env);
virtual ~SBPLEnvironment2D();
virtual DiscreteSpaceInformation * getDSI();
@@ -101,7 +102,6 @@
virtual void SetGoalTolerance(double tol_xy, double tol_th);
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
- virtual std::string getName() const;
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
@@ -122,10 +122,10 @@
public:
SBPLEnvironment3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV3DKIN * env,
footprint_t const & footprint,
double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos);
+ double timetoturn45degsinplace_secs);
virtual ~SBPLEnvironment3DKIN();
virtual DiscreteSpaceInformation * getDSI();
@@ -139,7 +139,6 @@
virtual void SetGoalTolerance(double tol_xy, double tol_th);
virtual deprecated_msgs::Pose2DFloat32 GetPoseFromState(int stateID) const throw(invalid_state);
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const;
- virtual std::string getName() const;
protected:
virtual bool DoUpdateCost(int ix, int iy, unsigned char newcost);
@@ -219,40 +218,78 @@
SBPLEnvironment * SBPLEnvironment::
create2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it)
+ boost::shared_ptr<IndexTransform const> it,
+ bool is16connected) throw(std::exception)
{
- SBPLEnvironment2D * env(new SBPLEnvironment2D(cm, it));
- return env;
+ EnvironmentNAV2D * env(new EnvironmentNAV2D());
+ if ((is16connected) && ( ! env->SetEnvParameter("is16connected", 1))) {
+ delete env;
+ throw runtime_error("mpglue::SBPLEnvironment::create2D(): EnvironmentNAV2D::SetEnvParameter() failed for \"is16connected\"");
+ }
+
+ int const obst_cost_thresh(cm->getCSpaceObstacleCost());
+
+ // good: Take advantage of the fact that InitializeEnv() can take
+ // a NULL-pointer as mapdata in order to initialize to all
+ // freespace.
+ //
+ // bad: Most costmaps do not support negative grid indices, so the
+ // generic CostmapAccessor::getXBegin() and getYBegin() are ignored
+ // and simply assumed to always return 0 (which they won't if we
+ // use growable costmaps).
+ env->InitializeEnv(cm->getXEnd(), // width
+ cm->getYEnd(), // height
+ 0, // mapdata
+ obst_cost_thresh);
+
+ // as above, assume getXBegin() and getYBegin() are always zero
+ for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
+ for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
+ int cost;
+ if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
+ env->UpdateCost(ix, iy, cost);
+ }
+
+ return new SBPLEnvironment2D(cm, it, env);
}
SBPLEnvironment * SBPLEnvironment::
create3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ // bool is16connected,
footprint_t const & footprint,
double nominalvel_mpersecs,
double timetoturn45degsinplace_secs,
- ostream * dbgos)
+ ostream * dbgos) throw(std::exception)
{
- SBPLEnvironment3DKIN * env(new SBPLEnvironment3DKIN(cm, it,
- footprint, nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- return env;
- }
-
-}
-
-namespace {
-
- SBPLEnvironment2D::
- SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it)
- : SBPLEnvironment(cm, it),
- env_(new EnvironmentNAV2D())
- {
- int const obst_cost_thresh(cm->getCSpaceObstacleCost());
+ EnvironmentNAV3DKIN * env(new EnvironmentNAV3DKIN());
+// if ((is16connected) && ( ! env->SetEnvParameter("is16connected", 1))) {
+// delete env;
+// throw runtime_error("mpglue::SBPLEnvironment::create3DKIN(): EnvironmentNAV3DKIN::SetEnvParameter() failed for \"is16connected\"");
+// }
+ int const obst_cost_thresh(cm->getWSpaceObstacleCost());
+ vector<sbpl_2Dpt_t> perimeterptsV;
+ perimeterptsV.reserve(footprint.size());
+ for (size_t ii(0); ii < footprint.size(); ++ii) {
+ sbpl_2Dpt_t pt;
+ pt.x = footprint[ii].x;
+ pt.y = footprint[ii].y;
+ perimeterptsV.push_back(pt);
+ }
+
+ if (dbgos) {
+ *dbgos << "mpglue::SBPLEnvironment3DKIN:\n"
+ << " perimeterptsV =\n";
+ for (vector<sbpl_2Dpt_t>::const_iterator ip(perimeterptsV.begin());
+ ip != perimeterptsV.end(); ++ip)
+ *dbgos << " " << ip->x << "\t" << ip->y << "\n";
+ *dbgos << " nominalvel_mpersecs = " << nominalvel_mpersecs << "\n"
+ << " timetoturn45degsinplace_secs = " << timetoturn45degsinplace_secs << "\n"
+ << " obst_cost_thresh = " << obst_cost_thresh << "\n" << flush;
+ }
+
// good: Take advantage of the fact that InitializeEnv() can take
// a NULL-pointer as mapdata in order to initialize to all
// freespace.
@@ -261,22 +298,39 @@
// generic CostmapAccessor::getXBegin() and getYBegin() are ignored
// and simply assumed to always return 0 (which they won't if we
// use growable costmaps).
- env_->InitializeEnv(cm->getXEnd(), // width
- cm->getYEnd(), // height
- 0, // mapdata
- obst_cost_thresh);
+ env->InitializeEnv(cm->getXEnd(), // width
+ cm->getYEnd(), // height
+ 0, // mapdata
+ perimeterptsV, it->getResolution(), nominalvel_mpersecs,
+ timetoturn45degsinplace_secs, obst_cost_thresh);
// as above, assume getXBegin() and getYBegin() are always zero
for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
int cost;
if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
- env_->UpdateCost(ix, iy, cost);
+ env->UpdateCost(ix, iy, cost);
}
+
+ return new SBPLEnvironment3DKIN(cm, it, env, footprint,
+ nominalvel_mpersecs, timetoturn45degsinplace_secs);
}
+}
+
+namespace {
SBPLEnvironment2D::
+ SBPLEnvironment2D(boost::shared_ptr<CostmapAccessor const> cm,
+ boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV2D * env)
+ : SBPLEnvironment(cm, it),
+ env_(env)
+ {
+ }
+
+
+ SBPLEnvironment2D::
~SBPLEnvironment2D()
{
delete env_;
@@ -396,70 +450,16 @@
}
- std::string SBPLEnvironment2D::
- getName() const
- {
- std::string name("2D");
- return name;
- }
-
-
SBPLEnvironment3DKIN::
SBPLEnvironment3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ EnvironmentNAV3DKIN * env,
footprint_t const & footprint,
double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
+ double timetoturn45degsinplace_secs)
: SBPLEnvironment(cm, it),
- env_(new EnvironmentNAV3DKIN())
+ env_(env)
{
- int const obst_cost_thresh(cm->getWSpaceObstacleCost());
- vector<sbpl_2Dpt_t> perimeterptsV;
- perimeterptsV.reserve(footprint.size());
- for (size_t ii(0); ii < footprint.size(); ++ii) {
- sbpl_2Dpt_t pt;
- pt.x = footprint[ii].x;
- pt.y = footprint[ii].y;
- perimeterptsV.push_back(pt);
- }
-
- if (dbgos) {
- *dbgos << "mpglue::SBPLEnvironment3DKIN:\n"
- << " perimeterptsV =\n";
- for (vector<sbpl_2Dpt_t>::const_iterator ip(perimeterptsV.begin());
- ip != perimeterptsV.end(); ++ip)
- *dbgos << " " << ip->x << "\t" << ip->y << "\n";
- *dbgos << " nominalvel_mpersecs = " << nominalvel_mpersecs << "\n"
- << " timetoturn45degsinplace_secs = " << timetoturn45degsinplace_secs << "\n"
- << " obst_cost_thresh = " << obst_cost_thresh << "\n" << flush;
- }
-
- // good: Take advantage of the fact that InitializeEnv() can take
- // a NULL-pointer as mapdata in order to initialize to all
- // freespace.
- //
- // bad: Most costmaps do not support negative grid indices, so the
- // generic CostmapAccessor::getXBegin() and getYBegin() are ignored
- // and simply assumed to always return 0 (which they won't if we
- // use growable costmaps).
- //
- // also there is quite a bit of code duplication between this, the
- // SBPLEnvironment2D ctor, and
- // SBPLEnvironment3DKIN::DoUpdateCost()...
- env_->InitializeEnv(cm->getXEnd(), // width
- cm->getYEnd(), // height
- 0, // mapdata
- perimeterptsV, it->getResolution(), nominalvel_mpersecs,
- timetoturn45degsinplace_secs, obst_cost_thresh);
-
- // as above, assume getXBegin() and getYBegin() are always zero
- for (ssize_t ix(0); ix < cm->getXEnd(); ++ix)
- for (ssize_t iy(0); iy < cm->getYEnd(); ++iy) {
- int cost;
- if (cm->getCost(ix, iy, &cost)) // "always" succeeds though
- env_->UpdateCost(ix, iy, cost);
- }
}
@@ -591,12 +591,4 @@
return env_->GetStateFromCoord(ix, iy, ith);
}
-
- std::string SBPLEnvironment3DKIN::
- getName() const
- {
- std::string name("3DKIN");
- return name;
- }
-
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_environment.h 2009-02-12 00:27:17 UTC (rev 11017)
@@ -83,14 +83,16 @@
static SBPLEnvironment * create2D(boost::shared_ptr<CostmapAccessor const> cm,
- boost::shared_ptr<IndexTransform const> it);
+ boost::shared_ptr<IndexTransform const> it,
+ bool is16connected) throw(std::exception);
static SBPLEnvironment * create3DKIN(boost::shared_ptr<CostmapAccessor const> cm,
boost::shared_ptr<IndexTransform const> it,
+ //bool is16connected,
footprint_t const & footprint,
double nominalvel_mpersecs,
double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+ std::ostream * dbgos) throw(std::exception);
virtual DiscreteSpaceInformation * getDSI() = 0;
virtual bool InitializeMDPCfg(MDPConfig *MDPCfg) = 0;
@@ -147,8 +149,6 @@
of the map. */
virtual int GetStateFromPose(deprecated_msgs::Pose2DFloat32 const & pose) const = 0;
- virtual std::string getName() const = 0;
-
boost::shared_ptr<CostmapAccessor const> getCostmap() const { return cm_; }
boost::shared_ptr<IndexTransform const> getIndexTransform() const { return it_; }
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_planner.cpp 2009-02-12 00:27:17 UTC (rev 11017)
@@ -194,65 +194,65 @@
}
- SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search)
- {
- shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
- shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search)
+// {
+// shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
+// shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search)
- {
- shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
- shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search)
+// {
+// shared_ptr<SBPLEnvironment> environment(SBPLEnvironment::create2D(cm, it));
+// shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
- {
- shared_ptr<SBPLEnvironment>
- environment(SBPLEnvironment::create3DKIN(cm, it,
- footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// ostream * dbgos)
+// {
+// shared_ptr<SBPLEnvironment>
+// environment(SBPLEnvironment::create3DKIN(cm, it,
+// footprint,
+// nominalvel_mpersecs,
+// timetoturn45degsinplace_secs,
+// dbgos));
+// shared_ptr<SBPLPlanner> planner(new ARAPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
- SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- ostream * dbgos)
- {
- shared_ptr<SBPLEnvironment>
- environment(SBPLEnvironment::create3DKIN(cm, it,
- footprint,
- nominalvel_mpersecs,
- timetoturn45degsinplace_secs,
- dbgos));
- shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
- SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
- return result;
- }
+// SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// ostream * dbgos)
+// {
+// shared_ptr<SBPLEnvironment>
+// environment(SBPLEnvironment::create3DKIN(cm, it,
+// footprint,
+// nominalvel_mpersecs,
+// timetoturn45degsinplace_secs,
+// dbgos));
+// shared_ptr<SBPLPlanner> planner(new ADPlanner(environment->getDSI(), forward_search));
+// SBPLPlannerWrap * result(new SBPLPlannerWrap(environment, planner));
+// return result;
+// }
}
Modified: pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h
===================================================================
--- pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h 2009-02-11 23:39:43 UTC (rev 11016)
+++ pkg/trunk/motion_planning/mpglue/src/sbpl_planner.h 2009-02-12 00:27:17 UTC (rev 11017)
@@ -89,33 +89,33 @@
};
- SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh);
+// SBPLPlannerWrap * createARAStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh);
- SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh);
+// SBPLPlannerWrap * createADStar2D(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh);
- SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+// SBPLPlannerWrap * createARAStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// std::ostream * dbgos);
- SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
- boost::shared_ptr<IndexTransform const> it,
- bool forward_search,
- int obst_cost_thresh,
- footprint_t const & footprint,
- double nominalvel_mpersecs,
- double timetoturn45degsinplace_secs,
- std::ostream * dbgos);
+// SBPLPlannerWrap * createADStar3DKIN(boost::shared_ptr<CostmapAccessor> cm,
+// boost::shared_ptr<IndexTransform const> it,
+// bool forward_search,
+// int obst_cost_thresh,
+// footprint_t const & footprint,
+// double nominalvel_mpersecs,
+// double timetoturn45degsinplace_secs,
+// std::ostream * dbgos);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-12 20:46:53
|
Revision: 11058
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11058&view=rev
Author: hsujohnhsu
Date: 2009-02-12 20:46:43 +0000 (Thu, 12 Feb 2009)
Log Message:
-----------
* arm_defs - increase damping on wrist_roll
* gripper_defs - update some mass/cg info. update collision boxes.
* test_arm.py - updates due to changes in gripper_defs
* test_camera.valid.ppm - new image
* set_goal.py - rostopic change for bumper feedback to /info (new /force)
* r_arm_grasping_demo.launch - new comments on rxplot
* urdf2gazebo and urdf2factory - fix axis offset, still off by a little bit, think it's gazebo error.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.valid.ppm
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Modified: pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/demos/arm_gazebo/r_arm_grasping_demo.launch 2009-02-12 20:46:43 UTC (rev 11058)
@@ -32,6 +32,7 @@
<!-- for visualization -->
<!--
<node pkg="ogre_visualizer" type="standalone_visualizer.py" respawn="false" output="screen" />
+ <node pkg="rosviz" type="rxplot" args="/r_gripper_l_finger_tip_bumper/force/vector/x /r_gripper_l_finger_tip_bumper/force/vector/y /r_gripper_l_finger_tip_bumper/force/vector/z" respawn="false" output="screen" />
-->
<param name="robotdesc/table" command="$(find xacro)/xacro.py '$(find examples_gazebo)/table_defs/table_defs.xml'" />
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-02-12 20:46:43 UTC (rev 11058)
@@ -163,8 +163,8 @@
pub_goal = rospy.Publisher("goal", Planner2DGoal) #received by wavefront_player or equivalent
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom" , RobotBase2DOdom , self.odomInput)
- rospy.Subscriber("base_bumper" , String , self.bumpedInput)
- rospy.Subscriber("torso_lift_bumper" , String , self.bumpedInput)
+ rospy.Subscriber("base_bumper/info" , String , self.bumpedInput)
+ rospy.Subscriber("torso_lift_bumper/info", String , self.bumpedInput)
rospy.init_node(NAME, anonymous=True)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-12 20:46:43 UTC (rev 11058)
@@ -353,7 +353,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 1.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
+ tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 1.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_arm.py 2009-02-12 20:46:43 UTC (rev 11058)
@@ -70,21 +70,21 @@
TEST_TIMEOUT = 50.0
# pre-recorded poses for above commands
-TARGET_PALM_TX = 0.714446037376
-TARGET_PALM_TY = 0.278116217261
-TARGET_PALM_TZ = 0.784828115329
-TARGET_PALM_QX = 0.237210502633
-TARGET_PALM_QY = -0.182803620068
-TARGET_PALM_QZ = 0.190038432015
-TARGET_PALM_QW = 0.934986314492
+TARGET_PALM_TX = 0.727619501795
+TARGET_PALM_TY = 0.313729662125
+TARGET_PALM_TZ = 0.789418474081
+TARGET_PALM_QX = 0.237324223096
+TARGET_PALM_QY = -0.182525634899
+TARGET_PALM_QZ = 0.190044686338
+TARGET_PALM_QW = 0.935010493487
-TARGET_FNGR_TX = 0.777019619684
-TARGET_FNGR_TY = 0.30750529852
-TARGET_FNGR_TZ = 0.822392652239
-TARGET_FNGR_QX = 0.201732745048
-TARGET_FNGR_QY = -0.221335744649
-TARGET_FNGR_QZ = 0.350039819038
-TARGET_FNGR_QW = 0.887573384461
+TARGET_FNGR_TX = 0.789451486586
+TARGET_FNGR_TY = 0.34264635999
+TARGET_FNGR_TZ = 0.826417972272
+TARGET_FNGR_QX = 0.207225356499
+TARGET_FNGR_QY = -0.216229383323
+TARGET_FNGR_QZ = 0.327829069225
+TARGET_FNGR_QW = 0.896008151069
class ArmTest(unittest.TestCase):
def __init__(self, *args):
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/test_camera.valid.ppm
===================================================================
(Binary files differ)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-02-12 20:46:43 UTC (rev 11058)
@@ -341,7 +341,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 1.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
+ tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 1.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-02-12 20:46:43 UTC (rev 11058)
@@ -569,7 +569,7 @@
<anchor xyz="0 0 0" />
<limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="2" />
<calibration reference_position="${1.27+wrist_roll_cal}" values="1.5 -1" />
- <joint_properties damping="0.1" />
+ <joint_properties damping="2.0" />
</joint>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-02-12 20:37:36 UTC (rev 11057)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-02-12 20:46:43 UTC (rev 11058)
@@ -6,7 +6,7 @@
<property name="M_PI" value="3.1415926535897931" />
<property name="gripper_max_angle" value="0.548" />
- <property name="gripper_min_angle" value="0.05" />
+ <property name="gripper_min_angle" value="0.00" />
<property name="finger_tip_mu" value="50.0" />
<property name="finger_joint_effort_limit" value="10.0" />
@@ -19,26 +19,30 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<calibration reference_position="0" values="1.0 -1.0" />
- <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle}" max="${gripper_max_angle}"
k_position="1.0" k_velocity="1.0" velocity="0.5"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
<map name="${prefix}_l_finger_joint_feedback" flag="gazebo">
<elem key="provideFeedback" value="true"/>
+ <elem key="initialAngle" value="${gripper_min_angle}"/>
</map>
</joint>
<link name="${prefix}_l_finger_link">
<parent name="${parent}" />
- <origin xyz="0.07751 ${1*0.01} 0" rpy="0 0 ${gripper_min_angle}" />
+ <origin xyz="0.07691 0.01 0" rpy="0 0 0" />
<joint name="${prefix}_l_finger_joint" />
<inertial>
<mass value="0.17126" />
- <com xyz="0.03598 0.01730 -0.00164" />
- <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385"
- iyy="0.00019708305" iyz="-0.00000306125"
- izz="0.00018105446" />
+ <com xyz="${0.03598} 0.01730 -0.00164" />
+ <inertia ixx= "0.00007756198"
+ ixy= "0.00000149095"
+ ixz="-0.00000983385"
+ iyy= "0.00019708305"
+ iyz="-0.00000306125"
+ izz= "0.00018105446" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -50,9 +54,9 @@
</geometry>
</visual>
<collision>
- <origin xyz="0.045 0.015 0.0" rpy="0 0 0" />
+ <origin xyz="0.03598 0.01730 -0.00164" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_collision">
- <box size="0.09 0.030 0.049" />
+ <box size="0.09 0.040 0.049" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -86,7 +90,7 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<calibration reference_position="0" values="1.0 -1.0" />
- <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle}"
k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.2" />
@@ -96,20 +100,24 @@
<elem key="mimicKd" value="10.0" />
<elem key="mimicFMax" value="10.0" />
<elem key="provideFeedback" value="true"/>
+ <elem key="initialAngle" value="${-gripper_min_angle}"/>
</map>
</joint>
<link name="${prefix}_r_finger_link">
<parent name="${parent}" />
- <origin xyz="0.07751 ${-1*0.01} 0" rpy="0 0 ${-gripper_min_angle}" />
+ <origin xyz="0.07691 -0.01 0" rpy="0 0 0" />
<joint name="${prefix}_r_finger_joint" />
<inertial>
<mass value="0.17389" />
- <com xyz="0.03576 -0.01736 -0.00095" />
- <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228"
- iyy="0.00019847383" iyz="0.00000246110"
- izz="0.00018106988" />
+ <com xyz="${0.03576} -0.01736 -0.00095" />
+ <inertia ixx= "0.00007738410"
+ ixy="-0.00000209309"
+ ixz="-0.00000836228"
+ iyy= "0.00019847383"
+ iyz= "0.00000246110"
+ izz= "0.00018106988" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -121,9 +129,9 @@
</geometry>
</visual>
<collision>
- <origin xyz="0.045 -0.015 0.0" rpy="0 0 0" />
+ <origin xyz="0.03576 -0.01736 -0.00095" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_collision">
- <box size="0.09 0.030 0.049" />
+ <box size="0.09 0.040 0.049" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -157,7 +165,7 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<calibration reference_position="0" values="1.0 -1.0" />
- <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle*0}"
+ <limit effort="${finger_joint_effort_limit}" min="${-gripper_max_angle}" max="${-gripper_min_angle}"
k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
@@ -167,12 +175,13 @@
<elem key="mimicKd" value="10.0" />
<elem key="mimicFMax" value="10.0" />
<elem key="provideFeedback" value="true"/>
+ <elem key="initialAngle" value="${-gripper_min_angle}"/>
</map>
</joint>
<link name="${prefix}_l_finger_tip_link">
<parent name="${prefix}_l_finger_link" />
- <origin xyz="0.0915 0 0" rpy="0 0 ${-gripper_min_angle}" />
+ <origin xyz="0.09137 0.00495 0" rpy="0 0 0" />
<joint name="${prefix}_l_finger_tip_joint" />
<inertial>
@@ -183,7 +192,7 @@
izz="0.00001541768" />
</inertial>
<visual>
- <origin xyz="0 0 0 " rpy="0 0 0 " />
+ <origin xyz="0 0 0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
<elem key="material" value="Gazebo/Green" />
</map>
@@ -192,9 +201,9 @@
</geometry>
</visual>
<collision>
- <origin xyz="0.0120 0.005 0.0" rpy="0 0 0" />
+ <origin xyz="0.00846 0.00568 0.0" rpy="0 0 0" />
<geometry name="${prefix}_l_finger_tip_collision">
- <box size="0.038 0.04 0.023" />
+ <box size="0.04577 0.0425 0.023" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -221,6 +230,7 @@
<elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
+ <elem key="selfCollide" value="false" />
</map>
</link>
@@ -230,7 +240,7 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<calibration reference_position="0" values="1.0 -1.0" />
- <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle*0}" max="${gripper_max_angle}"
+ <limit effort="${finger_joint_effort_limit}" min="${gripper_min_angle}" max="${gripper_max_angle}"
k_position="0" k_velocity="0" velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<joint_properties damping="0.002" />
@@ -240,17 +250,18 @@
<elem key="mimicKd" value="10.0" />
<elem key="mimicFMax" value="10.0" />
<elem key="provideFeedback" value="true"/>
+ <elem key="initialAngle" value="${gripper_min_angle}"/>
</map>
</joint>
<link name="${prefix}_r_finger_tip_link">
<parent name="${prefix}_r_finger_link" />
- <origin xyz="0.0915 0 0" rpy="0 0 ${gripper_min_angle}" />
+ <origin xyz="${0.09137} ${-0.00495} 0" rpy="0 0 0" />
<joint name="${prefix}_r_finger_tip_joint" />
<inertial>
<mass value="0.04419" />
- <com xyz="0.00423 -0.00284 0.0" />
+ <com xyz="${0.00423} ${-0.00284} ${0.0}" />
<inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0"
iyy="0.00000987067" iyz="0.0"
izz="0.00001541768" />
@@ -265,9 +276,9 @@
</geometry>
</visual>
<collision>
- <origin xyz="0.0120 -0.005 0.0" rpy="0 0 0" />
+ <origin xyz="${0.00846} ${-0.00568} 0.0" rpy="0 0 0" />
<geometry name="${prefix}_r_finger_tip_collision">
- <box size="0.038 0.04 0.023" />
+ <box size="0.04577 0.0425 0.023" />
</geometry>
<verbose value="Yes" />
<map flag="collision" name="mesh">
@@ -294,6 +305,7 @@
<elem key="mu2" value="${finger_tip_mu}" />
<elem key="kp" value="100000000.0" />
<elem key="kd" value="1.0" />
+ <elem key="selfCollide" value="false" />
</map>
</link>
@@ -469,9 +481,18 @@
<com xyz="0 0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material" value="Gazebo/Shell" />
+ </map>
+ <geometry name="${side}_gripper_tool_visual">
+ <mesh scale="0.0 0.0 0.0" />
+ </geometry>
+ </visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_collision_dummy">
+ <geometry name="${side}_gripper_tool_collision">
<box size="0.1 0.1 0.1" />
</geometry>
</collision>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-13 00:06:07
|
Revision: 11069
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11069&view=rev
Author: hsujohnhsu
Date: 2009-02-13 00:06:04 +0000 (Fri, 13 Feb 2009)
Log Message:
-----------
correct cg location is relative to geom, not body, so subtrace collision origin from com-xyz.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-12 23:58:06 UTC (rev 11068)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-13 00:06:04 UTC (rev 11069)
@@ -225,7 +225,11 @@
static const char tagList2[3][3] = {"cx", "cy", "cz"};
for (int j = 0 ; j < 3 ; ++j)
- addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
+ {
+ // by doing this we support only 1 geom
+ double tmp_value = (link->inertial->com)[j] - (link->collision->xyz)[j];
+ addKeyValue(geom, tagList2[j], values2str(1, &tmp_value));
+ }
if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
{
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-02-12 23:58:06 UTC (rev 11068)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2009-02-13 00:06:04 UTC (rev 11069)
@@ -213,7 +213,11 @@
static const char tagList2[3][3] = {"cx", "cy", "cz"};
for (int j = 0 ; j < 3 ; ++j)
- addKeyValue(geom, tagList2[j], values2str(1, link->inertial->com + j));
+ {
+ // by doing this we support only 1 geom
+ double tmp_value = (link->inertial->com)[j] - (link->collision->xyz)[j];
+ addKeyValue(geom, tagList2[j], values2str(1, &tmp_value));
+ }
if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-02-13 02:45:15
|
Revision: 11079
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11079&view=rev
Author: hsujohnhsu
Date: 2009-02-13 02:45:05 +0000 (Fri, 13 Feb 2009)
Log Message:
-----------
consolidating urdf2gazebo and urdf2factory:
* gazebo_plugin/lib/liburdf2gazebo.so - library
* gazebo_plugin/urdf2file - convert urdf to gazebo file
* gazebo_plugin/urdf2factory - spawn robot in gazebo from parameter server urdf string
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2file.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
Removed Paths:
-------------
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2009-02-13 02:13:21 UTC (rev 11078)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2009-02-13 02:45:05 UTC (rev 11079)
@@ -36,4 +36,10 @@
rospack_add_library(ros_bumper src/ros_bumper.cpp)
rospack_link_boost(ros_bumper thread)
+rospack_add_library(urdf2gazebo src/urdf2gazebo.cpp)
+
rospack_add_executable(urdf2factory src/urdf2factory.cpp)
+target_link_libraries(urdf2factory urdf2gazebo)
+
+rospack_add_executable(urdf2file src/urdf2file.cpp)
+target_link_libraries(urdf2file urdf2gazebo)
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/urdf2gazebo.h 2009-02-13 02:45:05 UTC (rev 11079)
@@ -0,0 +1,78 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+#ifndef URDF2GAZEBO_HH
+#define URDF2GAZEBO_HH
+
+#include <cstdio>
+#include <cstdlib>
+#include <cmath>
+
+#include <vector>
+#include <string>
+
+#include <sstream>
+
+#include <urdf/URDF.h>
+
+#include "LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+
+namespace urdf2gazebo
+{
+ double rad2deg(double v) { return v * 180.0 / M_PI; };
+ class URDF2Gazebo
+ {
+ public:
+ URDF2Gazebo();
+ ~URDF2Gazebo();
+
+ std::string values2str(unsigned int count, const double *values, double (*conv)(double));
+
+ void setupTransform(btTransform &transform, const double *xyz, const double *rpy);
+
+ void addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value);
+
+ void addTransform(TiXmlElement *elem, const::btTransform& transform);
+
+ void copyGazeboMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags);
+
+ std::string getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals);
+
+ void convertLink(TiXmlElement *root, robot_desc::URDF::Link *link, const btTransform &transform, bool enforce_limits);
+
+ void convert(robot_desc::URDF &wgxml, TiXmlDocument &doc, bool enforce_limits);
+ };
+}
+
+#endif
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-13 02:13:21 UTC (rev 11078)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cpp 2009-02-13 02:45:05 UTC (rev 11079)
@@ -34,11 +34,8 @@
#include <cstdio>
#include <cstdlib>
-#include <cmath>
-#include <vector>
#include <string>
-
#include <sstream>
#include <gazebo/gazebo.h>
@@ -48,391 +45,10 @@
#include <urdf/URDF.h>
-#include "LinearMath/btTransform.h"
-#include "LinearMath/btVector3.h"
+#include <gazebo_plugin/urdf2gazebo.h>
-std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL)
-{
- std::stringstream ss;
- for (unsigned int i = 0 ; i < count ; i++)
- {
- if (i > 0)
- ss << " ";
- ss << (conv ? conv(values[i]) : values[i]);
- }
- return ss.str();
-}
+using namespace urdf2gazebo;
-double rad2deg(double v)
-{
- return v * 180.0 / M_PI;
-}
-
-void setupTransform(btTransform &transform, const double *xyz, const double *rpy)
-{
- btMatrix3x3 mat;
- mat.setEulerZYX(rpy[0],rpy[1],rpy[2]);
- transform = btTransform(mat,btVector3(xyz[0],xyz[1],xyz[2]));
-}
-
-void addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value)
-{
- TiXmlElement *ekey = new TiXmlElement(key);
- TiXmlText *text_ekey = new TiXmlText(value);
- ekey->LinkEndChild(text_ekey);
- elem->LinkEndChild(ekey);
-}
-
-void addTransform(TiXmlElement *elem, const::btTransform& transform)
-{
- btVector3 pz = transform.getOrigin();
- double cpos[3] = { pz.x(), pz.y(), pz.z() };
- btMatrix3x3 mat = transform.getBasis();
- double crot[3];
- mat.getEulerZYX(crot[0],crot[1],crot[2]);
-
- /* set geometry transform */
- addKeyValue(elem, "xyz", values2str(3, cpos));
- addKeyValue(elem, "rpy", values2str(3, crot, rad2deg));
-}
-
-void copyGazeboMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags = NULL)
-{
- std::vector<std::string> gazebo_names;
- data.getMapTagNames("gazebo", gazebo_names);
- for (unsigned int k = 0 ; k < gazebo_names.size() ; ++k)
- {
- std::map<std::string, std::string> m = data.getMapTagValues("gazebo", gazebo_names[k]);
- std::vector<std::string> accepted_tags;
- if (tags)
- accepted_tags = *tags;
- else
- for (std::map<std::string, std::string>::iterator it = m.begin() ; it != m.end() ; it++)
- accepted_tags.push_back(it->first);
-
- for (unsigned int i = 0 ; i < accepted_tags.size() ; ++i)
- if (m.find(accepted_tags[i]) != m.end())
- addKeyValue(elem, accepted_tags[i], m[accepted_tags[i]]);
-
- std::map<std::string, const TiXmlElement*> x = data.getMapTagXML("gazebo", gazebo_names[k]);
- for (std::map<std::string, const TiXmlElement*>::iterator it = x.begin() ; it != x.end() ; it++)
- {
- for (const TiXmlNode *child = it->second->FirstChild() ; child ; child = child->NextSibling())
- elem->LinkEndChild(child->Clone());
- }
- }
-}
-
-std::string getGeometrySize(robot_desc::URDF::Link::Geometry* geometry, int *sizeCount, double *sizeVals)
-{
- std::string type;
-
- switch (geometry->type)
- {
- case robot_desc::URDF::Link::Geometry::BOX:
- type = "box";
- *sizeCount = 3;
- {
- robot_desc::URDF::Link::Geometry::Box* box = static_cast<robot_desc::URDF::Link::Geometry::Box*>(geometry->shape);
- sizeVals[0] = box->size[0];
- sizeVals[1] = box->size[1];
- sizeVals[2] = box->size[2];
- }
- break;
- case robot_desc::URDF::Link::Geometry::CYLINDER:
- type = "cylinder";
- *sizeCount = 2;
- {
- robot_desc::URDF::Link::Geometry::Cylinder* cylinder = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(geometry->shape);
- sizeVals[0] = cylinder->radius;
- sizeVals[1] = cylinder->length;
- }
- break;
- case robot_desc::URDF::Link::Geometry::SPHERE:
- type = "sphere";
- *sizeCount = 1;
- sizeVals[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(geometry->shape)->radius;
- break;
- case robot_desc::URDF::Link::Geometry::MESH:
- type = "trimesh";
- *sizeCount = 3;
- {
- robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(geometry->shape);
- sizeVals[0] = mesh->scale[0];
- sizeVals[1] = mesh->scale[1];
- sizeVals[2] = mesh->scale[2];
- }
- break;
- default:
- *sizeCount = 0;
- printf("Unknown body type: %d in geometry '%s'\n", geometry->type, geometry->name.c_str());
- break;
- }
-
- return type;
-}
-
-void convertLink(TiXmlElement *root, robot_desc::URDF::Link *link, const btTransform &transform, bool enforce_limits)
-{
- btTransform currentTransform = transform;
-
- int linkGeomSize;
- double linkSize[3];
- std::string type = getGeometrySize(link->collision->geometry, &linkGeomSize, linkSize);
-
- // This should be made smarter.
- if(!link->visual)
- {
- printf("ignoring link without visual tag: %s\n", link->name.c_str());
- return;
- }
-
- if (!type.empty())
- {
- /* create new body */
- TiXmlElement *elem = new TiXmlElement("body:" + type);
-
- /* set body name */
- elem->SetAttribute("name", link->name);
-
- /* compute global transform */
- btTransform localTransform;
- setupTransform(localTransform, link->xyz, link->rpy);
- currentTransform *= localTransform;
- addTransform(elem, currentTransform);
-
- if (link->collision->verbose)
- addKeyValue(elem, "reportStaticCollision", "true");
-
- /* begin create geometry node */
- TiXmlElement *geom = new TiXmlElement("geom:" + type);
-
- {
- /* set its name */
- geom->SetAttribute("name", link->collision->geometry->name);
-
- /* set transform */
- addKeyValue(geom, "xyz", values2str(3, link->collision->xyz));
- addKeyValue(geom, "rpy", values2str(3, link->collision->rpy, rad2deg));
-
- /* set mass properties */
- addKeyValue(geom, "massMatrix", "true");
- addKeyValue(geom, "mass", values2str(1, &link->inertial->mass));
-
- static const char tagList1[6][4] = {"ixx", "ixy", "ixz", "iyy", "iyz", "izz"};
- for (int j = 0 ; j < 6 ; ++j)
- addKeyValue(geom, tagList1[j], values2str(1, link->inertial->inertia + j));
-
- static const char tagList2[3][3] = {"cx", "cy", "cz"};
- for (int j = 0 ; j < 3 ; ++j)
- {
- // by doing this we support only 1 geom
- double tmp_value = (link->inertial->com)[j] - (link->collision->xyz)[j];
- addKeyValue(geom, tagList2[j], values2str(1, &tmp_value));
- }
-
- if (link->collision->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
- {
- robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->collision->geometry->shape);
- /* set mesh size or scale */
- addKeyValue(geom, "scale", values2str(3, mesh->scale));
-
- /* set mesh file */
- addKeyValue(geom, "mesh", "models/pr2/" + mesh->filename + ".mesh");
-
- }
- else
- {
- /* set geometry size */
- addKeyValue(geom, "size", values2str(linkGeomSize, linkSize));
- }
-
- /* set additional data */
- copyGazeboMap(link->collision->data, geom);
-
- /* begin create visual node */
- TiXmlElement *visual = new TiXmlElement("visual");
- {
- /* compute the visualisation transfrom */
- btTransform coll;
- setupTransform(coll, link->collision->xyz, link->collision->rpy);
- coll.inverse();
-
- btTransform vis;
- setupTransform(vis, link->visual->xyz, link->visual->rpy);
- coll = coll.inverseTimes(vis);
-
- addTransform(visual, coll);
-
- /* set geometry size */
-
- if (link->visual->geometry->type == robot_desc::URDF::Link::Geometry::MESH)
- {
- robot_desc::URDF::Link::Geometry::Mesh* mesh = static_cast<robot_desc::URDF::Link::Geometry::Mesh*>(link->visual->geometry->shape);
- /* set mesh size or scale */
- /*
- if (link->visual->geometry->isSet["size"])
- addKeyValue(visual, "size", values2str(3, mesh->size));
- else
- addKeyValue(visual, "scale", values2str(3, mesh->scale));
- */
- addKeyValue(visual, "scale", values2str(3, mesh->scale));
-
- /* set mesh file */
- if (mesh->filename.empty())
- addKeyValue(visual, "mesh", "unit_" + type);
- else
- addKeyValue(visual, "mesh", "models/pr2/" + mesh->filename + ".mesh");
-
- }
- else
- {
- int visualGeomSize;
- double visualSize[3];
- getGeometrySize(link->visual->geometry, &visualGeomSize, visualSize);
- addKeyValue(visual, "size", values2str(visualGeomSize, visualSize));
- }
-
- copyGazeboMap(link->visual->data, visual);
- }
- /* end create visual node */
-
- geom->LinkEndChild(visual);
- }
- /* end create geometry node */
-
- /* add geometry to body */
- elem->LinkEndChild(geom);
-
- /* copy gazebo data */
- copyGazeboMap(link->data, elem);
-
- /* add body to document */
- root->LinkEndChild(elem);
-
- /* compute the joint tag */
- bool fixed = false;
- std::string jtype;
- switch (link->joint->type)
- {
- case robot_desc::URDF::Link::Joint::REVOLUTE:
- jtype = "hinge";
- break;
- case robot_desc::URDF::Link::Joint::PRISMATIC:
- jtype = "slider";
- break;
- case robot_desc::URDF::Link::Joint::FLOATING:
- case robot_desc::URDF::Link::Joint::PLANAR:
- break;
- case robot_desc::URDF::Link::Joint::FIXED:
- jtype = "hinge";
- fixed = true;
- break;
- default:
- printf("Unknown joint type: %d in link '%s'\n", link->joint->type, link->name.c_str());
- break;
- }
-
- if (!jtype.empty())
- {
- TiXmlElement *joint = new TiXmlElement("joint:" + jtype);
- joint->SetAttribute("name", link->joint->name);
-
- addKeyValue(joint, "body1", link->name);
- addKeyValue(joint, "body2", link->parentName);
- addKeyValue(joint, "anchor", link->name);
-
- if (fixed)
- {
- addKeyValue(joint, "lowStop", "0");
- addKeyValue(joint, "highStop", "0");
- addKeyValue(joint, "axis", "1 0 0");
- }
- else
- {
- addKeyValue(joint, "axis", values2str(3, link->joint->axis));
-
- double tmpAnchor[3];
-
- for (int j = 0 ; j < 3 ; ++j)
- {
- // undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 1.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
- }
-
- addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
-
- if (enforce_limits && link->joint->isSet["limit"])
- {
- if (jtype == "slider")
- {
- addKeyValue(joint, "lowStop", values2str(1, link->joint->limit ));
- addKeyValue(joint, "highStop", values2str(1, link->joint->limit + 1 ));
- }
- else
- {
- addKeyValue(joint, "lowStop", values2str(1, link->joint->limit , rad2deg));
- addKeyValue(joint, "highStop", values2str(1, link->joint->limit + 1, rad2deg));
- }
- }
-
- if (link->joint->isSet["pjointMimic"])
- addKeyValue(joint,"mimicJoint", link->joint->pjointMimic->name );
- if (link->joint->isSet["fMimicMult"])
- addKeyValue(joint,"mimicMult", values2str(1, &(link->joint->fMimicMult) ));
- if (link->joint->isSet["fMimicOffset"])
- addKeyValue(joint,"mimicOffset", values2str(1, &(link->joint->fMimicOffset) ));
-
- }
-
- /* copy gazebo data */
- copyGazeboMap(link->joint->data, joint);
-
- /* add joint to document */
- root->LinkEndChild(joint);
- }
- }
-
- for (unsigned int i = 0 ; i < link->children.size() ; ++i)
- convertLink(root, link->children[i], currentTransform, enforce_limits);
-}
-
-void convert(robot_desc::URDF &wgxml, TiXmlDocument &doc, bool enforce_limits)
-{
- TiXmlDeclaration *decl = new TiXmlDeclaration("1.0", "", "");
- doc.LinkEndChild(decl);
-
- /* create root element and define needed namespaces */
- TiXmlElement *robot = new TiXmlElement("model:physical");
- robot->SetAttribute("xmlns:gazebo", "http://playerstage.sourceforge.net/gazebo/xmlschema/#gz");
- robot->SetAttribute("xmlns:model", "http://playerstage.sourceforge.net/gazebo/xmlschema/#model");
- robot->SetAttribute("xmlns:sensor", "http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor");
- robot->SetAttribute("xmlns:body", "http://playerstage.sourceforge.net/gazebo/xmlschema/#body");
- robot->SetAttribute("xmlns:geom", "http://playerstage.sourceforge.net/gazebo/xmlschema/#geom");
- robot->SetAttribute("xmlns:joint", "http://playerstage.sourceforge.net/gazebo/xmlschema/#joint");
- robot->SetAttribute("xmlns:controller", "http://playerstage.sourceforge.net/gazebo/xmlschema/#controller");
- robot->SetAttribute("xmlns:interface", "http://playerstage.sourceforge.net/gazebo/xmlschema/#interface");
- robot->SetAttribute("xmlns:rendering", "http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering");
- robot->SetAttribute("xmlns:physics", "http://playerstage.sourceforge.net/gazebo/xmlschema/#physics");
-
- // Create a node to enclose the robot body
- robot->SetAttribute("name", "pr2_model");
-
- /* set the transform for the whole model to identity */
- addKeyValue(robot, "xyz", "0 0 0");
- addKeyValue(robot, "rpy", "0 0 0");
- btTransform transform;
- transform.setIdentity();
-
- for (unsigned int k = 0 ; k < wgxml.getDisjointPartCount() ; ++k)
- convertLink(robot, wgxml.getDisjointPart(k), transform, enforce_limits);
-
- /* find all data item types */
- copyGazeboMap(wgxml.getMap(), robot);
-
- doc.LinkEndChild(robot);
-}
-
void usage(const char *progname)
{
printf("\nUsage: %s param_name [initial x y z roll pitch yaw]\n", progname);
@@ -509,7 +125,7 @@
std::string xml_content;
rosnode->getParam(argv[1],xml_content);
- // Parse URDF to get gazebo model.
+ // Parse URDF from param server
bool enforce_limits = true;
robot_desc::URDF wgxml;
if (!wgxml.loadString(xml_content.c_str()))
@@ -517,10 +133,16 @@
printf("Unable to load robot model from param server robotdesc/pr2\n");
exit(2);
}
- TiXmlDocument doc;
+
+ //
+ // init a parser library
+ //
+ URDF2Gazebo u2g;
// do the number crunching to make gazebo.model file
- convert(wgxml, doc, enforce_limits);
+ TiXmlDocument doc;
+ u2g.convert(wgxml, doc, enforce_limits);
+
//std::cout << " doc " << doc << std::endl << std::endl;
// copy model to a string
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2file.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2file.cpp (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2file.cpp 2009-02-13 02:45:05 UTC (rev 11079)
@@ -0,0 +1,88 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <cstdio>
+#include <cstdlib>
+#include <cmath>
+
+#include <urdf/URDF.h>
+
+#include <gazebo_plugin/urdf2gazebo.h>
+
+using namespace urdf2gazebo;
+
+void usage(const char *progname)
+{
+ printf("\nUsage: %s URDF.xml Gazebo.model\n", progname);
+ printf(" where URDF.xml is the file containing a robot description in the Willow Garage format (URDF)\n");
+ printf(" and Gazebo.model is the file where the Gazebo model should be written\n\n");
+}
+
+int main(int argc, char **argv)
+{
+ if (argc < 3)
+ {
+ usage(argv[0]);
+ exit(1);
+ }
+
+ // Experimental: take in optional argument to disable passing the limit for grasping test
+ bool enforce_limits = true;
+ if (argc == 4)
+ {
+ printf("Not enforcing limits\n");
+ enforce_limits = false;
+ }
+
+ robot_desc::URDF wgxml;
+
+ if (!wgxml.loadFile(argv[1]))
+ {
+ printf("Unable to load robot model from %s\n", argv[1]);
+ exit(2);
+ }
+
+ TiXmlDocument doc;
+
+ URDF2Gazebo u2g;
+ u2g.convert(wgxml, doc, enforce_limits);
+
+ if (!doc.SaveFile(argv[2]))
+ {
+ printf("Unable to save gazebo model in %s\n", argv[2]);
+ exit(3);
+ }
+
+ return 0;
+}
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-02-13 02:45:05 UTC (rev 11079)
@@ -0,0 +1,433 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <gazebo_plugin/urdf2gazebo.h>
+
+using namespace urdf2gazebo;
+
+URDF2Gazebo::URDF2Gazebo()
+{
+}
+
+URDF2Gazebo::~URDF2Gazebo()
+{
+}
+
+
+std::string URDF2Gazebo::values2str(unsigned int count, const double *values, double (*conv)(double) = NULL)
+{
+ std::stringstream ss;
+ for (unsigned int i = 0 ; i < count ; i++)
+ {
+ if (i > 0)
+ ss << " ";
+ ss << (conv ? conv(values[i]) : values[i]);
+ }
+ return ss.str();
+}
+
+void URDF2Gazebo::setupTransform(btTransform &transform, const double *xyz, const double *rpy)
+{
+ btMatrix3x3 mat;
+ mat.setEulerZYX(rpy[0],rpy[1],rpy[2]);
+ transform = btTransform(mat,btVector3(xyz[0],xyz[1],xyz[2]));
+}
+
+void URDF2Gazebo::addKeyValue(TiXmlElement *elem, const std::string& key, const std::string &value)
+{
+ TiXmlElement *ekey = new TiXmlElement(key);
+ TiXmlText *text_ekey = new TiXmlText(value);
+ ekey->LinkEndChild(text_ekey);
+ elem->LinkEndChild(ekey);
+}
+
+void URDF2Gazebo::addTransform(TiXmlElement *elem, const::btTransform& transform)
+{
+ btVector3 pz = transform.getOrigin();
+ double cpos[3] = { pz.x(), pz.y(), pz.z() };
+ btMatrix3x3 mat = transform.getBasis();
+ double crot[3];
+ mat.getEulerZYX(crot[0],crot[1],crot[2]);
+
+ /* set geometry transform */
+ addKeyValue(elem, "xyz", values2str(3, cpos));
+ addKeyValue(elem, "rpy", values2str(3, crot, rad2deg));
+}
+
+void URDF2Gazebo::copyGazeboMap(const robot_desc::URDF::Map& data, TiXmlElement *elem, const std::vector<std::string> *tags = NULL)
+{
+ std::vector<std::string> gazebo_names;
+ data.getMapTagNames("gazebo", gazebo_names);
+ for (unsigned int k = 0 ; k < gazebo_names.size() ; ++k)
+ {
+ std::map<std::string, std::string> m = data.getMapTagValues("gazebo", gazebo_names[k]);
+ std::vector<std::string> accepted_tags;
+ if (tags)
+ accepted_tags = *tags;
+ else
+ for (std::map<std::string, std::string>::iterator it = m.begin() ; it != m.end() ; it++)
+ accepted_tags.push_back(it->first);
+
+ for (unsigned int i = 0 ; i < accepted_tags.size() ; ++i)
+ if (m.find(accepted_tags[i]) != m.end())
+ addKeyValue(elem, accepted_tags[i], m[accepted_tags[...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-13 05:32:38
|
Revision: 11084
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11084&view=rev
Author: tfoote
Date: 2009-02-13 05:32:36 +0000 (Fri, 13 Feb 2009)
Log Message:
-----------
multi robot stage demo working
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -973,13 +973,13 @@
robot_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
out2.header.stamp.fromSec(time);
- out2.header.frame_id = "base_footprint";
- out2.parent_id = "base_link";
+ out2.header.frame_id = "base_link";
+ out2.parent_id = "base_footprint";
out2.transform.translation.x = 0;
out2.transform.translation.y = 0;
// FIXME: this is the offset between base_link origin and the ideal floor
- out2.transform.translation.z = -0.051; // FIXME: this is hardcoded, considering we are deprecating base_footprint soon, I will not get this from URDF.
+ out2.transform.translation.z = 0.051; // FIXME: this is hardcoded, considering we are deprecating base_footprint soon, I will not get this from URDF.
out2.transform.rotation.x = 0;
out2.transform.rotation.y = 0;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -1039,11 +1039,11 @@
robot_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
out2.header.stamp.fromSec(time);
- out2.header.frame_id = "base_footprint";
- out2.parent_id = "base_link";
+ out2.header.frame_id = "base_link";
+ out2.parent_id = "base_footprint";
out2.transform.translation.x = 0;
out2.transform.translation.y = 0;
- out2.transform.translation.z = -c_->wheel_radius_;
+ out2.transform.translation.z = c_->wheel_radius_;
out2.transform.rotation.x = 0;
out2.transform.rotation.y = 0;
out2.transform.rotation.z = 0;
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2009-02-13 05:32:36 UTC (rev 11084)
@@ -15,6 +15,7 @@
<!--<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full-0.05.pgm 0.05" respawn="false" />-->
<!-- localization settings -->
+ <param name="pf_odom_frame_id" value="odom_combined"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
<!-- filter out veil effect from range scans -->
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-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-02-13 05:32:36 UTC (rev 11084)
@@ -1,23 +1,24 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" output="screen" />
+ <node pkg="rosstage" type="rosstage" 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" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" >
- <param name="wavefront_player_0/__tf_prefix" value="robot_0" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
+ <param name="tf_prefix" value="robot_0"/>
<remap from="scan" to="base_scan" />
</node>
<node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_1" ns="robot_1" >
- <param name="wavefront_player_1/__tf_prefix" value="robot_1" />
+ <param name="tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
<remap from="scan" to="base_scan" />
</node>
- <node pkg="nav_view" type="nav_view" respawn="false"/>
+ <node pkg="nav_view" type="nav_view" respawn="false" ns="robot_1"/>
+ <node pkg="nav_view" type="nav_view" respawn="false" ns="robot_0"/>
<param name="max_publish_frequency" value="20.0"/>
- <node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_0" ns="robot_0" >
- <param name="fake_localization_0/__tf_prefix" value="robot_0" />
+ <node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_0" ns="robot_0" output="screen">
+ <param name="tf_prefix" value="robot_0" />
</node>
<node pkg="fake_localization" type="fake_localization" respawn="false" name="fake_localization_1" ns="robot_1" >
- <param name="fake_localization_1/__tf_prefix" value="robot_1" />
+ <param name="tf_prefix" value="robot_1" />
</node>
</group>
</launch>
Modified: pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
===================================================================
--- pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -145,8 +145,8 @@
m_model->getKmodelSimple()->computeTransforms(m_model->getRobotStateSimple()->getParams());
}
- if (cloud.header.frame_id != "map") {
- ROS_ERROR("Robot filter needs point clouds in the map frame. It was given a point cloud in the %s frame.",
+ if (cloud.header.frame_id != "/map") {
+ ROS_ERROR("Robot filter needs point clouds in the /map frame. It was given a point cloud in the %s frame.",
cloud.header.frame_id.c_str());
}
Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -345,7 +345,7 @@
my_filter_.getEstimate(ros::Time(), tmp);
if(!vo_active_)
tmp.getOrigin().setZ(0.0);
- odom_broadcaster_.sendTransform(Stamped<Transform>(tmp.inverse(), tmp.stamp_, publish_name, "base_footprint"));
+ odom_broadcaster_.sendTransform(Stamped<Transform>(tmp, tmp.stamp_, "base_footprint", publish_name));
#ifdef __EKF_DEBUG_FILE__
// write to file
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -224,7 +224,7 @@
robotPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", robotPose, globalPose);
+ tf_.transformPose("/map", robotPose, globalPose);
}
catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -91,7 +91,7 @@
double inscribedRadius(0.325);
double weight(0.1); // Scale costs down by a factor of 10
// Which frame is "global"
- param("/global_frame_id", global_frame_, std::string("map"));
+ param("/global_frame_id", global_frame_, std::string("/map"));
param("/costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -242,7 +242,7 @@
robot_srvs::StaticMap::Request req;
robot_srvs::StaticMap::Response resp;
puts("Requesting the map...");
- while(!ros::service::call("static_map", req, resp))
+ while(!ros::service::call("/static_map", req, resp))
{
puts("request failed; trying again...");
usleep(1000000);
@@ -491,6 +491,7 @@
t, "base_footprint"),odom_to_map);
}
catch(tf::TransformException e){
+ ROS_DEBUG("Dropping out of process message step\n");
return(0);
}
@@ -498,8 +499,8 @@
ros::Time transform_expiration;
transform_expiration.fromSec(hdr->timestamp + transform_tolerance_);
this->tf->sendTransform(tf::Stamped<tf::Transform> (tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
- tf::Point( odom_to_map.getOrigin() ) ),
- transform_expiration, "map",odom_frame_id));
+ tf::Point( odom_to_map.getOrigin() ) ).inverse(),
+ transform_expiration,odom_frame_id, "/map"));
/*
printf("lpose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
@@ -515,7 +516,7 @@
localizedOdomMsg.pos.y = pdata->pos.py;
localizedOdomMsg.pos.th = pdata->pos.pa;
localizedOdomMsg.header.stamp.fromSec(hdr->timestamp);
- localizedOdomMsg.header.frame_id = "map";
+ localizedOdomMsg.header.frame_id = "/map";
/*
printf("O: %.6f %.3f %.3f %.3f\n",
hdr->timestamp,
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -82,6 +82,8 @@
#include "ros/console.h"
#include "tf/transform_broadcaster.h"
+#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
class FakeOdomNode: public ros::Node
@@ -91,15 +93,18 @@
{
advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
-
m_tfServer = new tf::TransformBroadcaster(*this);
-
+ m_tfListener = new tf::TransformListener(*this);
m_lastUpdate = ros::Time::now();
m_base_pos_received = false;
+ param("pf_odom_frame_id", odom_frame_id_, std::string("odom"));
m_iniPos.x = m_iniPos.y = m_iniPos.th = 0.0;
m_particleCloud.set_particles_size(1);
+ notifier = new tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>(m_tfListener, this,
+ boost::bind(&FakeOdomNode::update, this, _1),
+ "base_pose_ground_truth_BOGUS(MANUALLY STUFFING)", odom_frame_id_, 100);
subscribe("base_pose_ground_truth", m_basePosMsg, &FakeOdomNode::basePosReceived,1);
}
@@ -114,16 +119,20 @@
void run(void)
{
// A duration for sleeping will be 100 ms
- ros::Duration snoozer(0, 100000000);
+ ros::Duration snoozer;
+ snoozer.fromSec(0.1);
while(true){
snoozer.sleep();
}
}
+
private:
-
tf::TransformBroadcaster *m_tfServer;
+ tf::TransformListener *m_tfListener;
+ tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>* notifier;
+
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
bool m_base_pos_received;
@@ -132,21 +141,28 @@
robot_msgs::ParticleCloud m_particleCloud;
deprecated_msgs::RobotBase2DOdom m_currentPos;
deprecated_msgs::Pose2DFloat32 m_iniPos;
+
+ //parameter for what odom to use
+ std::string odom_frame_id_;
- void basePosReceived(void)
- {
- update();
- }
+ void basePosReceived()
+ {
+ m_basePosMsg.header.frame_id = "base_footprint"; //hack to make the notifier do what I want (changed back later)
+ boost::shared_ptr<robot_msgs::PoseWithRatesStamped> message(new robot_msgs::PoseWithRatesStamped);
+ *message = m_basePosMsg;
+ notifier->enqueueMessage(message);
+ // update();
+ }
+public:
+ void update(const tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>::MessagePtr & message){
+ tf::Transform txi(tf::Quaternion(message->pos.orientation.x,
+ message->pos.orientation.y,
+ message->pos.orientation.z,
+ message->pos.orientation.w),
+ tf::Point(message->pos.position.x,
+ message->pos.position.y,
+ 0.0*message->pos.position.z )); // zero height for base_footprint
- void update(){
- tf::Transform txi(tf::Quaternion(m_basePosMsg.pos.orientation.x,
- m_basePosMsg.pos.orientation.y,
- m_basePosMsg.pos.orientation.z,
- m_basePosMsg.pos.orientation.w),
- tf::Point(m_basePosMsg.pos.position.x,
- m_basePosMsg.pos.position.y,
- 0.0*m_basePosMsg.pos.position.z )); // zero height for base_footprint
-
double x = txi.getOrigin().x() + m_iniPos.x;
double y = txi.getOrigin().y() + m_iniPos.y;
double z = txi.getOrigin().z();
@@ -165,15 +181,29 @@
// A hack links the two frames.
// m_tfServer->sendTransform(tf::Stamped<tf::Transform>
// (txIdentity.inverse(),
- // m_basePosMsg.header.stamp,
+ // message->header.stamp,
// "base_footprint", "base_link")); // this is published by base controller
+ // subtracting base to odom from map to base and send map to odom instead
+ tf::Stamped<tf::Pose> odom_to_map;
+ try
+ {
+ m_tfListener->transformPose(odom_frame_id_,tf::Stamped<tf::Pose> (txo.inverse(),
+ message->header.stamp, "base_footprint"),odom_to_map);
+ }
+ catch(tf::TransformException &e){
+ ROS_DEBUG("Failed to transform to odom %s\n",e.what());
+
+ return;
+ }
+
m_tfServer->sendTransform(tf::Stamped<tf::Transform>
- (txo.inverse(),
- m_basePosMsg.header.stamp,
- "map", "base_footprint"));
+ (odom_to_map.inverse(),
+ message->header.stamp,
+ odom_frame_id_, "/map"));
// Publish localized pose
- m_currentPos.header = m_basePosMsg.header;
+ m_currentPos.header = message->header;
+ m_currentPos.header.frame_id = "map"; ///\todo fixme hack
m_currentPos.pos.x = x;
m_currentPos.pos.y = y;
m_currentPos.pos.th = yaw;
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-02-13 05:32:36 UTC (rev 11084)
@@ -114,7 +114,7 @@
render_panel_->getViewport()->setCamera( camera_ );
- ros_node_->param("/global_frame_id", global_frame_id_, std::string("map"));
+ ros_node_->param("/global_frame_id", global_frame_id_, std::string("/map"));
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
ros_node_->advertise<deprecated_msgs::Pose2DFloat32>("initialpose", 1);
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -371,7 +371,7 @@
advertise<robot_msgs::PoseDot>("cmd_vel",1);
subscribe("goal", goalMsg, &WavefrontNode::goalReceived,1);
- scan_notifier = new tf::MessageNotifier<laser_scan::LaserScan>(&tf, this, boost::bind(&WavefrontNode::laserReceived, this, _1), "scan", "map", 1);
+ scan_notifier = new tf::MessageNotifier<laser_scan::LaserScan>(&tf, this, boost::bind(&WavefrontNode::laserReceived, this, _1), "scan", "/map", 1);
}
WavefrontNode::~WavefrontNode()
@@ -445,7 +445,7 @@
try
{
- this->tf.transformPointCloud("map", local_cloud, global_cloud);
+ this->tf.transformPointCloud("/map", local_cloud, global_cloud);
}
catch(tf::LookupException& ex)
{
@@ -577,7 +577,7 @@
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
- this->tf.transformPose("map", robotPose, global_pose);
+ this->tf.transformPose("/map", robotPose, global_pose);
}
catch(tf::LookupException& ex)
{
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2009-02-13 03:46:11 UTC (rev 11083)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2009-02-13 05:32:36 UTC (rev 11084)
@@ -167,7 +167,7 @@
if (positionmodels.size() > 1)
{
static char buf[100];
- snprintf(buf, sizeof(buf), "robot_%d/%s", robotID, name);
+ snprintf(buf, sizeof(buf), "/robot_%d/%s", robotID, name);
return buf;
}
else
@@ -336,8 +336,8 @@
// 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.inverse(),
- sim_time, mapName("base_footprint", r), mapName("base_link", r)));
+ (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;
@@ -356,8 +356,8 @@
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)).inverse(),
- sim_time, mapName("odom", r), mapName("base_link", r));
+ 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
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-02-17 06:08:57
|
Revision: 11242
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11242&view=rev
Author: rdiankov
Date: 2009-02-17 06:08:51 +0000 (Tue, 17 Feb 2009)
Log Message:
-----------
fixed openrave robot kinematics, ormanipulation plugin works again. Added auto-generation of inverse kinematics for pr2 using ikfast, openrave_robot_description now uses auto-generated ik
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
pkg/trunk/openrave_planning/orplugins/CMakeLists.txt
pkg/trunk/openrave_planning/orplugins/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/openrave_robot_description/src/ikbase.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/orrosikmain.cpp
pkg/trunk/robot_descriptions/openrave_robot_description/src/plugindefs.h
Removed Paths:
-------------
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-02-17 06:08:51 UTC (rev 11242)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 660
+SVN_REVISION = -r 686
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
#SVN_PATCH = fini_patch.patch
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/grasp_pr2_ricebox.mat
===================================================================
(Binary files differ)
Modified: pkg/trunk/openrave_planning/orplugins/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/orplugins/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orplugins/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -3,11 +3,13 @@
rospack(orplugins)
find_ros_package(orrosplanning)
+find_ros_package(openrave_robot_description)
file(GLOB orrosplanning_files ${orrosplanning_PACKAGE_PATH}/lib/*)
+file(GLOB openrave_robot_description_files ${openrave_robot_description_PACKAGE_PATH}/lib/*)
set(openrave_plugins )
-foreach(it ${orrosplanning_files})
+foreach(it ${orrosplanning_files} ${openrave_robot_description_files})
get_filename_component(basename ${it} NAME)
add_custom_command(
OUTPUT ${CMAKE_SOURCE_DIR}/${basename}
Modified: pkg/trunk/openrave_planning/orplugins/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orplugins/manifest.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orplugins/manifest.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -5,4 +5,5 @@
<author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
<depend package="orrosplanning"/>
+ <depend package="openrave_robot_description"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -5,5 +5,5 @@
rospack_add_boost_directories()
-rospack_add_library(orrosplanning src/orrosplanningmain.cpp src/rosarmik.cpp)
+rospack_add_library(orrosplanning src/orrosplanningmain.cpp)
rospack_link_boost(orrosplanning thread)
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -7,7 +7,6 @@
<depend package="roscpp"/>
<depend package="openrave"/>
<depend package="openrave_robot_description"/>
- <depend package="libKinematics"/>
<depend package="checkerboard_detector"/>
<depend package="robot_msgs"/>
<depend package="pr2_mechanism_controllers"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -25,7 +25,6 @@
// \author Rosen Diankov
#include "plugindefs.h"
-#include "rosarmik.h"
#include "mocapsystem.h"
#include "objecttransformsystem.h"
#include "collisionmapsystem.h"
@@ -58,10 +57,6 @@
if( wcsicmp(name, L"ROSRobot") == 0 )
return new ROSRobotController(penv);
break;
- case PT_InverseKinematicsSolver:
- if( wcsicmp(name, L"ROSArmIK") == 0 )
- return new ROSArmIK(penv);
- break;
case PT_ProblemInstance:
if( wcsicmp(name, L"ROSPlanning") == 0 )
return new ROSPlanningProblem(penv);
@@ -90,7 +85,6 @@
}
pinfo->controllers.push_back(L"ROSRobot");
- pinfo->iksolvers.push_back(L"ROSArmIK");
pinfo->problems.push_back(L"ROSPlanning");
pinfo->sensorsystems.push_back(L"ROSMocap");
pinfo->sensorsystems.push_back(L"ObjectTransform");
Deleted: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-02-17 06:08:51 UTC (rev 11242)
@@ -1,501 +0,0 @@
-// Software License Agreement (BSD License)
-// Copyright (c) 2008, Rosen Diankov
-// 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.
-// * The name of the author may not 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: Rosen Diankov
-#include "plugindefs.h"
-
-#include "rosarmik.h"
-
-#ifndef SQR
-template <class T>
-inline T SQR(T x) { return x * x; }
-#endif
-
-ROSArmIK::ROSArmIK(EnvironmentBase* penv) : IkSolverBase(penv)
-{
-}
-
-bool ROSArmIK::Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options)
-{
- assert( probot != NULL );
- _probot = probot;
- if( _probot == NULL || pmanip == NULL )
- return false;
-
- // get the joint limits
- const vector<int>& vjoints = pmanip->_vecarmjoints;
-
- vector<dReal> qlower, qupper;
- _probot->GetJointLimits(qlower, qupper);
- _qlower.resize(vjoints.size()); _qupper.resize(vjoints.size());
-
- for(int i = 0; i < (int)vjoints.size(); ++i) {
- _qlower[i] = qlower[vjoints[i]];
- _qupper[i] = qupper[vjoints[i]];
- }
-
- if( _qlower.size() > 0 )
- fiFreeParam = 1.0f / (_qupper[2]-_qlower[2]);
- else fiFreeParam = 1;
-
- std::vector<NEWMAT::Matrix> axis;
- std::vector<NEWMAT::Matrix> anchor;
- std::vector<std::string> joint_type;
-
- NEWMAT::Matrix aj(3,1);
- NEWMAT::Matrix an(3,1);
-
- joint_type.resize(7);
-
- // Shoulder pan
- aj << 0 << 0 << 1.0;
- axis.push_back(aj);
- an << 0 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Shoulder pitch
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.1 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Shoulder roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.1 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Elbow flex
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.5 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- // Forearm roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.5 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(-1);
-
- // Wrist flex
- aj << 0 << 1 << 0;
- axis.push_back(aj);
- an << 0.82025 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(-1);
-
- // Gripper roll
- aj << 1 << 0 << 0;
- axis.push_back(aj);
- an << 0.82025 << 0 << 0;
- anchor.push_back(an);
- _vjointmult.push_back(1);
-
- for(int i=0; i < 7; i++)
- joint_type[i] = std::string("ROTARY");
-
- iksolver.reset(new kinematics::arm7DOF(anchor,axis,joint_type));
-
- if( pmanip->_vecarmjoints.size() > 0 ) {
- RobotBase::RobotStateSaver saver(_probot);
- _probot->SetTransform(Transform());
- vector<dReal> vjoints(_probot->GetDOF(),0);
- _probot->SetJointValues(NULL,NULL,&vjoints[0]);
- Transform tbase = pmanip->pBase->GetTransform();
- KinBody::Joint* pjoint = _probot->GetJoint(pmanip->_vecarmjoints.front());
- KinBody::Link* pother = pjoint->GetFirstAttached() == pmanip->pBase ? pjoint->GetSecondAttached() : pjoint->GetFirstAttached();
- if( pother != NULL )
- voffset = tbase.trans - pother->GetTransform().trans;
- }
-
- return true;
-}
-
-// end eff transform is the transform of the wrist with respect to the base arm link
-bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, bool bCheckEnvCollision, dReal* qResult)
-{
- assert( _probot != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- assert( pmanip != NULL );
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() == _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- dReal startphi = q0 != NULL ? q0[2] : 0;
- dReal upperphi = _qupper[2], lowerphi = _qlower[2], deltaphi = 0;
- int iter = 0;
- bool bsuccess = false;
-
- dReal bestdist = 1000; // only valid if q0 != NULL
- NEWMAT::Matrix nmT = GetNewMat(_T);
-
- while(1) {
-
- dReal curphi = startphi;
- if( iter & 1 ) { // increment
- curphi += deltaphi;
- if( curphi > upperphi ) {
-
- if( startphi-deltaphi < lowerphi)
- break; // reached limit
- ++iter;
- continue;
- }
- }
- else { // decrement
- curphi -= deltaphi;
- if( curphi < lowerphi ) {
-
- if( startphi+deltaphi > upperphi )
- break; // reached limit
- deltaphi += GetPhiInc(); // increment
- ++iter;
- continue;
- }
-
- deltaphi += GetPhiInc(); // increment
- }
-
- iter++;
-
- iksolver->ComputeIKEfficientTheta3(nmT,curphi);
- vector<dReal> vravesol(_probot->GetActiveDOF());
- vector<dReal> vbest;
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solution that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- COLLISIONREPORT report;
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot, &report) ) {
- if( report.plink1 != NULL && report.plink2 != NULL ) {
- RAVELOG_VERBOSEA("WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
- }
- continue;
- }
-
- // solution is valid, check with q0
- if( q0 != NULL ) {
-
- dReal d = 0;
- for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(vravesol[k]-q0[k]);
-
- if( bestdist > d ) {
- vbest = vravesol;
- bestdist = d;
- }
- }
- else {
- vbest = vravesol;
- break;
- }
- }
-
- // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
- // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
- if( vbest.size() == _qlower.size() && qResult != NULL ) {
- memcpy(&qResult[0], &vbest[0], sizeof(dReal)*_qlower.size());
- bsuccess = true;
- break;
- }
- }
-
- return bsuccess;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
-{
- assert( _probot != NULL );
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
-
- assert( pmanip != NULL );
-
- qSolutions.resize(0);
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() && _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- double startphi = 0;
- double upperphi = _qupper[2], lowerphi = _qlower[2], deltaphi = 0;
- int iter = 0;
-
- NEWMAT::Matrix nmT = GetNewMat(_T);
-
- while(1) {
- double curphi = startphi;
- if( iter & 1 ) { // increment
- curphi += deltaphi;
- if( curphi > upperphi ) {
-
- if( startphi-deltaphi < lowerphi)
- break; // reached limit
- ++iter;
- continue;
- }
- }
- else { // decrement
- curphi -= deltaphi;
- if( curphi < lowerphi ) {
-
- if( startphi+deltaphi > upperphi )
- break; // reached limit
- ++iter;
- deltaphi += GetPhiInc(); // increment
- continue;
- }
-
- deltaphi += GetPhiInc(); // increment
- }
-
- iter++;
-
- iksolver->ComputeIKEfficientTheta3(nmT,curphi);
- vector<dReal> vravesol(_probot->GetActiveDOF());
-
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot) )
- continue;
-
- qSolutions.push_back(vravesol);
- }
- }
-
- return qSolutions.size()>0;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, const dReal* q0, const dReal* pFreeParameters,
- bool bCheckEnvCollision, dReal* qResult)
-{
- if( pFreeParameters == NULL )
- return Solve(_T, q0, bCheckEnvCollision, qResult);
-
- assert( _probot != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- assert( pmanip != NULL );
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() == _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- dReal bestdist = 1000; // only valid if q0 != NULL
-
- NEWMAT::Matrix nmT = GetNewMat(_T);
- iksolver->ComputeIKEfficientTheta3(nmT,_qlower[2] + (_qupper[2]-_qlower[2])*pFreeParameters[0]);
-
- vector<dReal> vravesol(_probot->GetActiveDOF());
- vector<dReal> vbest;
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i] * _vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions (does WAM ever self-collide?)
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- COLLISIONREPORT report;
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot, &report) ) {
- if( report.plink1 != NULL && report.plink2 != NULL ) {
- RAVELOG_VERBOSEA("WAMIK: collision %S:%S with %S:%S\n", report.plink1->GetParent()->GetName(), report.plink1->GetName(), report.plink2->GetParent()->GetName(), report.plink2->GetName());
- }
- continue;
- }
-
- // solution is valid, check with q0
- if( q0 != NULL ) {
-
- dReal d = 0;
-
- for(int k = 0; k < (int)pmanip->_vecarmjoints.size(); ++k)
- d += SQR(vravesol[k]-q0[k]);
-
- if( bestdist > d ) {
- vbest = vravesol;
- bestdist = d;
- }
- }
- else {
- vbest = vravesol;
- break;
- }
- }
-
- // return as soon as a solution is found, since we're visiting phis starting from q0[0], we are guaranteed
- // that the solution will be close (ie, phi's dominate in the search). This is to speed things up
- if( vbest.size() == _qlower.size() && qResult != NULL ) {
- memcpy(&qResult[0], &vbest[0], sizeof(dReal)*_qlower.size());
- return true;
- }
-
- return false;
-}
-
-bool ROSArmIK::Solve(const Transform &_T, const dReal* pFreeParameters,
- bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions)
-{
- if( pFreeParameters == NULL )
- return Solve(_T, bCheckEnvCollision, qSolutions);
-
- assert( _probot != NULL );
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
-
- assert( pmanip != NULL );
-
- qSolutions.resize(0);
-
- // the world coordinate system is at the origin of the intersection of the first 3 joint axes
- assert( pmanip->_vecarmjoints.size() && _qlower.size() );
-
- RobotBase::RobotStateSaver saver(_probot);
-
- _probot->SetActiveDOFs(pmanip->_vecarmjoints);
- vector<dReal> vravesol(_probot->GetActiveDOF());
-
- // start searching for phi close to q0, as soon as a solution is found for the curphi, return it
- NEWMAT::Matrix nmT = GetNewMat(_T);
- iksolver->ComputeIKEfficientTheta3(nmT,_qlower[2] + (_qupper[2]-_qlower[2])*pFreeParameters[0]);
-
- FOREACH(itsol, iksolver->solution_ik_) {
- vector<double>& sol = *itsol;
- assert( (int)sol.size() == _probot->GetActiveDOF());
-
- for(int i = 0; i < (int)sol.size(); ++i)
- vravesol[i] = sol[i]*_vjointmult[i];
-
- // find the first valid solutino that satisfies joint constraints and collisions
- if( !checkjointangles(vravesol) )
- continue;
-
- // check for self collisions
- _probot->SetActiveDOFValues(NULL, &vravesol[0]);
-
- if( _probot->CheckSelfCollision() )
- continue;
-
- if( bCheckEnvCollision && GetEnv()->CheckCollision(_probot) )
- continue;
-
- qSolutions.push_back(vravesol);
- }
-
- return qSolutions.size()>0;
-}
-
-bool ROSArmIK::GetFreeParameters(dReal* pFreeParameters) const
-{
- assert( _probot != NULL && pFreeParameters != NULL );
-
- const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
- if( pmanip == NULL )
- return false;
- assert( _qlower.size() > 2 && _qupper.size() > 2 );
- assert( pmanip->_vecarmjoints.size() > 0 && pmanip->_vecarmjoints[2] < _probot->GetDOF());
-
- dReal values[3];
- _probot->GetJointFromDOFIndex(pmanip->_vecarmjoints[2])->GetValues(values);
- pFreeParameters[0] = (values[0]-_qlower[2])*fiFreeParam;
- return true;
-}
-
-bool ROSArmIK::checkjointangles(vector<dReal>& vravesol)
-{
- for(int j = 0; j < (int)_qlower.size(); ++j) {
- if( _qlower[j] < -PI && vravesol[j] > _qupper[j] )
- vravesol[j] -= 2*PI;
- if( _qupper[j] > PI && vravesol[j] < _qlower[j] )
- vravesol[j] += 2*PI;
- if( vravesol[j] < _qlower[j] || vravesol[j] > _qupper[j] )
- return false;
- }
-
- return true;
-}
-
-NEWMAT::Matrix ROSArmIK::GetNewMat(const TransformMatrix& tm)
-{
- NEWMAT::Matrix nmT(4,4);
- nmT(1,1) = tm.m[0]; nmT(1,2) = tm.m[1]; nmT(1,3) = tm.m[2]; nmT(1,4) = tm.trans.x+voffset.x;
- nmT(2,1) = tm.m[4]; nmT(2,2) = tm.m[5]; nmT(2,3) = tm.m[6]; nmT(2,4) = tm.trans.y+voffset.y;
- nmT(3,1) = tm.m[8]; nmT(3,2) = tm.m[9]; nmT(3,3) = tm.m[10]; nmT(3,4) = tm.trans.z+voffset.z;
- nmT(4,1) = 0; nmT(4,2) = 0; nmT(4,3) = 0; nmT(4,4) = 1;
- return nmT;
-}
Deleted: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.h 2009-02-17 06:08:51 UTC (rev 11242)
@@ -1,64 +0,0 @@
-// Software License Agreement (BSD License)
-// Copyright (c) 2008, Rosen Diankov
-// 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.
-// * The name of the author may not 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: Rosen Diankov
-#ifndef OPENRAVE_ROSARMIK_H
-#define OPENRAVE_ROSARMIK_H
-
-#include <libKinematics/pr2_ik.h>
-
-class ROSArmIK : public IkSolverBase
-{
-public:
- ROSArmIK(EnvironmentBase* penv);
-
- virtual bool Init(RobotBase* probot, const RobotBase::Manipulator* pmanip, int options);
-
- virtual bool Solve(const Transform &transEE, const dReal* q0, bool bCheckEnvCollision, dReal* qResult);
- virtual bool Solve(const Transform &transEE, bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions);
-
- virtual bool Solve(const Transform &endEffTransform, const dReal* q0, const dReal* pFreeParameters,
- bool bCheckEnvCollision, dReal* qResult);
- virtual bool Solve(const Transform &endEffTransform, const dReal* pFreeParameters,
- bool bCheckEnvCollision, std::vector< std::vector<dReal> >& qSolutions);
-
- virtual int GetNumFreeParameters() const { return 1; }
- virtual bool GetFreeParameters(dReal* pFreeParameters) const;
- virtual RobotBase* GetRobot() const { return _probot; }
-
-private:
-
- NEWMAT::Matrix GetNewMat(const TransformMatrix& tm);
-
- inline dReal GetPhiInc() { return 0.03f; }
- bool checkjointangles(vector<dReal>& vravesol);
-
- RobotBase* _probot;
- std::vector<dReal> _qlower, _qupper, _vjointmult;
- Vector voffset;
- dReal fiFreeParam;
- boost::shared_ptr<kinematics::arm7DOF> iksolver;
-};
-
-#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2009-02-17 06:08:51 UTC (rev 11242)
@@ -42,6 +42,32 @@
add_custom_target(urdf_robots ALL DEPENDS ${robot_files})
add_dependencies(urdf_robots urdf2openrave)
+set(PR2_ROBOT "${CMAKE_SOURCE_DIR}/robots/pr2full.robot.xml")
+set(PR2_IK_LEFT_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/src/ik_pr2left.h")
+add_custom_command(
+ OUTPUT ${PR2_IK_LEFT_OUTPUT}
+ # need -d 0 to suppress spurious warnings
+ COMMAND "${openrave_PACKAGE_PATH}/bin/openrave" -d 0 --generateik robot "${PR2_ROBOT}" freeparam l_shoulder_pan_joint manipulator 0 output "${PR2_IK_LEFT_OUTPUT}"
+ DEPENDS ${PR2_ROBOT}
+ )
+add_custom_target(pr2leftik ALL DEPENDS ${PR2_IK_LEFT_OUTPUT})
+add_dependencies(pr2leftik urdf_robots)
+
+set(PR2_IK_RIGHT_OUTPUT "${CMAKE_CURRENT_SOURCE_DIR}/src/ik_pr2right.h")
+add_custom_command(
+ OUTPUT ${PR2_IK_RIGHT_OUTPUT}
+ # need -d 0 to suppress spurious warnings
+ COMMAND "${openrave_PACKAGE_PATH}/bin/openrave" -d 0 --generateik robot "${PR2_ROBOT}" freeparam r_shoulder_pan_joint manipulator 1 output "${PR2_IK_RIGHT_OUTPUT}"
+ DEPENDS ${PR2_ROBOT}
+ )
+add_custom_target(pr2rightik ALL DEPENDS ${PR2_IK_RIGHT_OUTPUT})
+add_dependencies(pr2rightik urdf_robots)
+
+# add the plugin for the inverse kinematics
+rospack_add_library(orrosik src/orrosikmain.cpp)
+add_dependencies(orrosik pr2leftik pr2rightik)
+#rospack_link_boost(orrosik thread)
+
# add the test package
rospack_add_gtest(test_or_robots test/test_robots.cpp)
target_link_libraries (test_or_robots openrave-core)
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-02-17 05:43:25 UTC (rev 11241)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/robots/pr2full.robot.xml 2009-02-17 06:08:51 UTC (rev 11242)
@@ -6,10 +6,10 @@
<base>torso_lift_link</base>
<effector>l_gripper_palm_link</effector>
<armjoints>l_shoulder_pan_joint l_shoulder_lift_joint l_upper_arm_roll_joint l_elbow_flex_joint l_forearm_roll_joint l_wrist_flex_joint l_wrist_roll_joint</armjoints>
- <joints>l_gripper_palm_joint l_gripper_l_finger_joint</joints>
+ <joints>l_gripper_palm_joint l_gripper_l_finger_joint l_gripper_tool_joint</joints>
<closed>0 0</closed>
<opened>0 0.8</opened>
- <iksolver>rosarmik</iksolver>
+ <iksolver>PR2Leftikfast</iksolver>
</Manipulator>
<!-- right arm -->
@@ -18,10 +18,10 @@
<base>torso_lift_link</base>
<effector>r_gripper_palm_link</effector>
<armjoints>r_shoulder_pan_joint r_shoulder_lift_joint r_upper_arm_roll_joint r_elbow_flex_joint r_forearm_roll_joint r_wrist_flex_joint r_wrist_roll_joint</armjoints>
- <joints>r_gripper_palm_joint r_gripper_l_finger_joint</joints>
...
[truncated message content] |
|
From: <is...@us...> - 2009-02-18 00:02:18
|
Revision: 11280
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11280&view=rev
Author: isucan
Date: 2009-02-18 00:02:13 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
easier to use state params, plan_kinematic_path prints the value of a joint
Modified Paths:
--------------
pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
pkg/trunk/highlevel/highlevel_controllers/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/test_path_execution.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Modified: pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
===================================================================
--- pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-18 00:02:13 UTC (rev 11280)
@@ -264,11 +264,11 @@
double pos = m_mechanismState.joint_states[i].position;
//printf("%d: %s\n", i, m_mechanismState.joint_states[i].name.c_str());
if (m_mechanismState.joint_states[i].name == "base_joint" && m_haveBasePos) {
- m_robotState->setParams(m_basePos, m_mechanismState.joint_states[i].name);
+ m_robotState->setParamsJoint(m_basePos, m_mechanismState.joint_states[i].name);
}
else
{
- m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ m_robotState->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
}
}
}
@@ -278,7 +278,7 @@
for (unsigned int i = 0 ; i < n ; ++i)
{
double pos = m_mechanismState.joint_states[i].position;
- m_robotStateSimple->setParams(&pos, m_mechanismState.joint_states[i].name);
+ m_robotStateSimple->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
}
}
m_haveMechanismState = true;
@@ -293,7 +293,7 @@
planning_models::KinematicModel::PlanarJoint* pj =
dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(m_kmodel->getRobot(i)->chain);
if (pj)
- m_robotState->setParams(m_basePos, pj->name);
+ m_robotState->setParamsJoint(m_basePos, pj->name);
}
stateUpdate();
}
Modified: pkg/trunk/highlevel/highlevel_controllers/manifest.xml
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/highlevel/highlevel_controllers/manifest.xml 2009-02-18 00:02:13 UTC (rev 11280)
@@ -34,6 +34,7 @@
<depend package="angles"/>
<depend package="robot_filter"/>
<depend package="wg_robot_description_parser"/>
+ <depend package="planning_models"/>
<depend package="libsunflower"/> <!-- mpglue also already depends on it though -->
<export>
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm2.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -246,16 +246,7 @@
std::string group, std::vector<double>& conf) {
conf.clear();
//Copy the stateparams in to the req.
- unsigned int len = model->getGroupDimension(model->getGroupID(group));
- double* param = new double[len];
- state->copyParams(param, model->getGroupID(group));
-
- for (unsigned int i = 0; i < len; i++) {
- //ROS_INFO("%f", param[i]);
- conf.push_back(param[i]);
- }
-
- delete[] param;
+ state->copyParamsGroup(conf, group);
}
@@ -291,7 +282,7 @@
ROS_ASSERT(axes == 1);
double* param = new double[axes];
param[0] = goalMsg.configuration[i].position;
- state->setParams(param, goalMsg.configuration[i].name);
+ state->setParamsJoint(param, goalMsg.configuration[i].name);
delete[] param;
}
goalMsg.unlock();
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-18 00:02:13 UTC (rev 11280)
@@ -240,7 +240,10 @@
planning_models::KinematicModel::PlanarJoint* pj =
dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(m_kmodel->getRobot(i)->chain);
if (pj)
- change = change || m_robotState->setParams(m_basePos, pj->name);
+ {
+ bool this_changed = m_robotState->setParamsJoint(m_basePos, pj->name);
+ change = change || this_changed;
+ }
}
if (change)
stateUpdate();
@@ -276,7 +279,7 @@
if (joint->usedParams == 1)
{
double pos = m_mechanismState.joint_states[i].position;
- bool this_changed = m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ bool this_changed = m_robotState->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
change = change || this_changed;
}
// else
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -209,7 +209,7 @@
// get the current params for the robot's right arm
double cmd[7];
- m_robotState->copyParams(cmd, m_kmodel->getGroupID(GROUPNAME));
+ m_robotState->copyParamsGroup(cmd, GROUPNAME);
robot_msgs::KinematicPath stop_path;
stop_path.set_states_size(1);
@@ -245,6 +245,7 @@
void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
{
sendDisplay(path, model);
+ printPath(path);
robot_msgs::JointTraj traj;
getTrajectoryMsg(path, traj);
m_node->publish("right_arm_trajectory_command", traj);
@@ -262,6 +263,18 @@
ROS_INFO("Sent planned path to display");
}
+ void printPath(robot_msgs::KinematicPath &path)
+ {
+ printf("Path with %d states", (int)path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < path.states[i].vals.size() ; ++j)
+ printf("%f ", path.states[i].vals[j]);
+ printf("\n");
+ }
+ printf("\n");
+ }
+
robot_msgs::KinematicPlanStatus plan_status_;
int plan_id_;
bool robot_stopped_;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_plan_to_state.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -136,6 +136,9 @@
if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
{
+ // print the path on screen
+ printPath(s_res.value.path);
+
// send the path to the visualizer
sendDisplay(req.start_state, s_res.value.path, GROUPNAME);
@@ -231,6 +234,18 @@
ROS_INFO("Sent planned path to display");
}
+ void printPath(robot_msgs::KinematicPath &path)
+ {
+ printf("Path with %d states", (int)path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < path.states[i].vals.size() ; ++j)
+ printf("%f ", path.states[i].vals[j]);
+ printf("\n");
+ }
+ printf("\n");
+ }
+
// check if straight line path is valid (motion_validator node)
bool verifyDirectPath(robot_msgs::KinematicState &start, robot_msgs::KinematicConstraints &cstrs,
robot_msgs::KinematicState &goal, const std::string &model)
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/execute_replan_to_state.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -137,7 +137,7 @@
// get the current params for the robot's right arm
double cmd[7];
- m_robotState->copyParams(cmd, m_kmodel->getGroupID(GROUPNAME));
+ m_robotState->copyParamsGroup(cmd, GROUPNAME);
robot_msgs::KinematicPath stop_path;
stop_path.set_states_size(1);
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/test_path_execution.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/right_arm/test_path_execution.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -95,9 +95,9 @@
double to_send[controllerDim];
for (int i = 0 ; i < controllerDim ; ++i)
to_send[i] = 0.0;
- sp->setParams(&val, joints[j]);
- sp->copyParams(to_send, m_kmodel->getGroupID(groupName));
- int index = sp->getPos(joints[j], m_kmodel->getGroupID(groupName));
+ sp->setParamsJoint(&val, joints[j]);
+ sp->copyParamsGroup(to_send, groupName);
+ int index = sp->getJointIndexInGroup(joints[j], groupName);
for (unsigned int i = 0 ; i < traj.points[0].get_positions_size() ; ++i)
traj.points[0].positions[i] = to_send[i];
@@ -140,7 +140,7 @@
printf("%f ", traj.points[0].positions[i]);
printf("\n");
- m_robotState->copyParams(to_send, m_kmodel->getGroupID(groupName));
+ m_robotState->copyParamsGroup(to_send, groupName);
printf("Achieved: ");
for (unsigned int i = 0 ; i < traj.points[0].get_positions_size() ; ++i)
printf("%f ", to_send[i]);
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -155,7 +155,7 @@
if (joint->usedParams == 1)
{
double pos = m_mechanismState.joint_states[i].position;
- bool this_changed = m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ bool this_changed = m_robotState->setParamsJoint(&pos, m_mechanismState.joint_states[i].name);
change = change || this_changed;
}
// else
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-02-18 00:02:13 UTC (rev 11280)
@@ -493,47 +493,109 @@
/** Mark all values as unseen */
void reset(void);
+ /** Mark all values in a group as unseen */
+ void resetGroup(int groupID);
+
+ /** Mark all values in a group as unseen */
+ void resetGroup(const std::string &group);
+
/** Set all the parameters to a given value */
void setAll(const double value);
+ /** Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, const std::string &group);
+
+ /** Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, int groupID);
+
/** Set all planar & floating joints to 0, so that the robot is in its own frame */
void setInRobotFrame(void);
+ /** Set the parameters for the complete robot. */
+ bool setParams(const std::vector<double> ¶ms);
+
+ /** Set the parameters for the complete robot. */
+ bool setParams(const double *params);
+
/** Set the parameters for a given group. Return true if
any change was observed in either of the set
values. */
- bool setParams(const double *params, int groupID = -1);
+ bool setParamsGroup(const std::vector<double> ¶ms, const std::string &group);
+
+ /** Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const std::vector<double> ¶ms, int groupID);
+ /** Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, const std::string &group);
+
+ /** Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, int groupID);
+
/** Given the name of a joint, set the values of the
parameters describing the joint. Return true if any
change was observed in the set value */
- bool setParams(const double *params, const std::string &name);
+ bool setParamsJoint(const double *params, const std::string &name);
+ /** Given the name of a joint, set the values of the
+ parameters describing the joint. Return true if any
+ change was observed in the set value */
+ bool setParamsJoint(const std::vector<double> ¶ms, const std::string &name);
+
/** Given the name of a joint, get the values of the
parameters describing the joint. */
- const double* getParams(const std::string &name);
+ const double* getParamsJoint(const std::string &name) const;
/** Return the current value of the params */
const double* getParams(void) const;
/** Get the offset for the parameter of a joint in a given group */
- int getPos(const std::string &name, int groupID = -1) const;
+ int getJointIndexInGroup(const std::string &name, const std::string &group) const;
+ /** Get the offset for the parameter of a joint in a given group */
+ int getJointIndexInGroup(const std::string &name, int groupID) const;
+
/** Copy the parameters for a given group to a destination address */
- void copyParams(double *params, int groupID = -1) const;
+ void copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const;
+
+ /** Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(std::vector<double> ¶ms, int groupID) const;
+
+ /** Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, const std::string &group) const;
+
+ /** Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, int groupID) const;
+ /** Copy all parameters to a destination address */
+ void copyParams(double *params) const;
+
+ /** Copy all parameters to a destination address */
+ void copyParams(std::vector<double> ¶ms) const;
+
/** Copy the parameters describen a given joint */
- void copyParams(double *params, const std::string &name) const;
+ void copyParamsJoint(double *params, const std::string &name) const;
+ /** Check if all params in a group were seen */
+ bool seenAllGroup(const std::string &group) const;
+
+ /** Check if all params in a group were seen */
+ bool seenAllGroup(int groupID) const;
+
/** Check if all params were seen */
- bool seenAll(int groupID = -1);
+ bool seenAll(void) const;
+
+ /** Print the data from the state to screen */
+ void print(std::ostream &out = std::cout) const;
/** Print the missing joint names */
void missing(int groupID = -1, std::ostream &out = std::cout);
- /** Print the data from the state to screen */
- void print(std::ostream &out = std::cout);
-
protected:
KinematicModel *m_owner;
Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -599,24 +599,59 @@
void planning_models::KinematicModel::StateParams::reset(void)
{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- m_seen[i] = false;
+ resetGroup(-1);
}
-bool planning_models::KinematicModel::StateParams::seenAll(int groupID)
+void planning_models::KinematicModel::StateParams::resetGroup(const std::string &group)
{
+ resetGroup(m_owner->getGroupID(group));
+}
+
+void planning_models::KinematicModel::StateParams::resetGroup(int groupID)
+{
if (groupID < 0)
+ {
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ m_seen[i] = false;
+ }
+ else
{
+ for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+ {
+ unsigned int j = m_mi.groupStateIndexList[groupID][i];
+ m_seen[j] = false;
+ }
+ }
+}
+
+bool planning_models::KinematicModel::StateParams::seenAll(void) const
+{
+ return seenAllGroup(-1);
+}
+
+bool planning_models::KinematicModel::StateParams::seenAllGroup(const std::string &group) const
+{
+ return seenAllGroup(m_owner->getGroupID(group));
+}
+
+bool planning_models::KinematicModel::StateParams::seenAllGroup(int groupID) const
+{
+ if (groupID < 0)
+ {
for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
- if (!m_seen[i])
+ {
+ std::map<unsigned int, bool>::const_iterator it = m_seen.find(i);
+ if (!it->second)
return false;
+ }
}
else
{
for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
{
unsigned int j = m_mi.groupStateIndexList[groupID][i];
- if (!m_seen[j])
+ std::map<unsigned int, bool>::const_iterator it = m_seen.find(j);
+ if (!it->second)
return false;
}
}
@@ -642,24 +677,40 @@
}
}
-const double* planning_models::KinematicModel::StateParams::getParams(const std::string &name)
+const double* planning_models::KinematicModel::StateParams::getParamsJoint(const std::string &name) const
{
Joint *joint = m_owner->getJoint(name);
if (joint)
{
- unsigned int pos = m_mi.parameterIndex[name];
- return m_params + pos;
+ std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
+ if (it != m_mi.parameterIndex.end())
+ return m_params + it->second;
+ else
+ return NULL;
}
else
return NULL;
}
-bool planning_models::KinematicModel::StateParams::setParams(const double *params, const std::string &name)
+bool planning_models::KinematicModel::StateParams::setParamsJoint(const std::vector<double> ¶ms, const std::string &name)
{
bool result = false;
Joint *joint = m_owner->getJoint(name);
if (joint)
{
+ double *dparams = new double[joint->usedParams];
+ result = setParamsJoint(dparams, name);
+ delete[] dparams;
+ }
+ return result;
+}
+
+bool planning_models::KinematicModel::StateParams::setParamsJoint(const double *params, const std::string &name)
+{
+ bool result = false;
+ Joint *joint = m_owner->getJoint(name);
+ if (joint)
+ {
unsigned int pos = m_mi.parameterIndex[name];
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
{
@@ -676,9 +727,39 @@
std::cerr << "Unknown joint: '" << name << "'" << std::endl;
return result;
}
-
-bool planning_models::KinematicModel::StateParams::setParams(const double *params, int groupID)
+
+bool planning_models::KinematicModel::StateParams::setParams(const std::vector<double> ¶ms)
{
+ return setParamsGroup(params, -1);
+}
+
+bool planning_models::KinematicModel::StateParams::setParams(const double *params)
+{
+ return setParamsGroup(params, -1);
+}
+
+bool planning_models::KinematicModel::StateParams::setParamsGroup(const std::vector<double> ¶ms, const std::string &group)
+{
+ return setParamsGroup(params, m_owner->getGroupID(group));
+}
+
+bool planning_models::KinematicModel::StateParams::setParamsGroup(const std::vector<double> ¶ms, int groupID)
+{
+ double *dparams = new double[m_owner->getGroupDimension(groupID)];
+ for (unsigned int i = 0 ; i < params.size() ; ++i)
+ dparams[i] = params[i];
+ bool result = setParamsGroup(dparams, groupID);
+ delete[] dparams;
+ return result;
+}
+
+bool planning_models::KinematicModel::StateParams::setParamsGroup(const double *params, const std::string &group)
+{
+ return setParamsGroup(params, m_owner->getGroupID(group));
+}
+
+bool planning_models::KinematicModel::StateParams::setParamsGroup(const double *params, int groupID)
+{
bool result = false;
if (groupID < 0)
{
@@ -706,27 +787,49 @@
return result;
}
-void planning_models::KinematicModel::StateParams::setAll(const double value)
+void planning_models::KinematicModel::StateParams::setAllInGroup(const double value, const std::string &group)
{
- for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ setAllInGroup(value, m_owner->getGroupID(group));
+}
+
+void planning_models::KinematicModel::StateParams::setAllInGroup(const double value, int groupID)
+{
+ if (groupID < 0)
{
- m_params[i] = value;
- m_seen[i] = true;
- }
+ for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
+ {
+ m_params[i] = value;
+ m_seen[i] = true;
+ }
+ }
+ else
+ {
+ for (unsigned int i = 0 ; i < m_mi.groupStateIndexList[groupID].size() ; ++i)
+ {
+ unsigned int j = m_mi.groupStateIndexList[groupID][i];
+ m_params[j] = value;
+ m_seen[j] = true;
+ }
+ }
}
+void planning_models::KinematicModel::StateParams::setAll(const double value)
+{
+ setAllInGroup(value, -1);
+}
+
void planning_models::KinematicModel::StateParams::setInRobotFrame(void)
{
for (unsigned int j = 0 ; j < m_mi.floatingJoints.size() ; ++j)
{
double vals[7] = {0, 0, 0, 0, 0, 0, 1};
- setParams(vals, m_mi.parameterName[m_mi.floatingJoints[j]]);
+ setParamsJoint(vals, m_mi.parameterName[m_mi.floatingJoints[j]]);
}
for (unsigned int j = 0 ; j < m_mi.planarJoints.size() ; ++j)
{
double vals[3] = {0, 0, 0};
- setParams(vals, m_mi.parameterName[m_mi.planarJoints[j]]);
+ setParamsJoint(vals, m_mi.parameterName[m_mi.planarJoints[j]]);
}
}
@@ -735,8 +838,13 @@
return m_params;
}
-int planning_models::KinematicModel::StateParams::getPos(const std::string &name, int groupID) const
+int planning_models::KinematicModel::StateParams::getJointIndexInGroup(const std::string &name, const std::string &group) const
{
+ return getJointIndexInGroup(name, m_owner->getGroupID(group));
+}
+
+int planning_models::KinematicModel::StateParams::getJointIndexInGroup(const std::string &name, int groupID) const
+{
Joint *joint = m_owner->getJoint(name);
if (joint)
{
@@ -757,7 +865,7 @@
return -1;
}
-void planning_models::KinematicModel::StateParams::copyParams(double *params, const std::string &name) const
+void planning_models::KinematicModel::StateParams::copyParamsJoint(double *params, const std::string &name) const
{
Joint *joint = m_owner->getJoint(name);
if (joint)
@@ -774,8 +882,39 @@
std::cerr << "Unknown joint: '" << name << "'" << std::endl;
}
-void planning_models::KinematicModel::StateParams::copyParams(double *params, int groupID) const
+void planning_models::KinematicModel::StateParams::copyParams(double *params) const
{
+ copyParamsGroup(params, -1);
+}
+
+void planning_models::KinematicModel::StateParams::copyParams(std::vector<double> ¶ms) const
+{
+ copyParamsGroup(params, -1);
+}
+
+void planning_models::KinematicModel::StateParams::copyParamsGroup(double *params, const std::string &group) const
+{
+ copyParamsGroup(params, m_owner->getGroupID(group));
+}
+
+void planning_models::KinematicModel::StateParams::copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const
+{
+ copyParamsGroup(params, m_owner->getGroupID(group));
+}
+
+void planning_models::KinematicModel::StateParams::copyParamsGroup(std::vector<double> ¶ms, int groupID) const
+{
+ unsigned int dim = m_owner->getGroupDimension(groupID);
+ double *dparams = new double[dim];
+ copyParamsGroup(dparams, groupID);
+ params.resize(dim);
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ params[i] = dparams[i];
+ delete[] dparams;
+}
+
+void planning_models::KinematicModel::StateParams::copyParamsGroup(double *params, int groupID) const
+{
if (groupID < 0)
{
for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)
@@ -867,7 +1006,7 @@
}
}
-void planning_models::KinematicModel::StateParams::print(std::ostream &out)
+void planning_models::KinematicModel::StateParams::print(std::ostream &out) const
{
out << std::endl;
for (std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.begin() ; it != m_mi.parameterIndex.end() ; ++it)
@@ -875,8 +1014,9 @@
Joint* joint = m_owner->getJoint(it->first);
if (joint)
{
- out << it->first;
- if (!m_seen[m_mi.parameterIndex[it->first]])
+ out << it->first;
+ std::map<unsigned int, bool>::const_iterator sit = m_seen.find(it->second);
+ if (!sit->second)
out << "[ *** UNSEEN *** ]";
out << ": ";
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
Modified: pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp 2009-02-17 23:56:29 UTC (rev 11279)
+++ pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp 2009-02-18 00:02:13 UTC (rev 11280)
@@ -428,15 +428,15 @@
double tmpParam[3];
tmpParam[0] = 0.1;
- sp->setParams(tmpParam, "link_a_joint");
+ sp->setParamsJoint(tmpParam, "link_a_joint");
EXPECT_FALSE(sp->seenAll());
tmpParam[0] = -1.0;
- sp->setParams(tmpParam, "link_c_joint");
+ sp->setParamsJoint(tmpParam, "link_c_joint");
EXPECT_FALSE(sp->seenAll());
tmpParam[0] = 0.5; tmpParam[1] = 0.4; tmpParam[2] = 1.1;
- sp->setParams(tmpParam, "base_link_joint");
+ sp->setParamsJoint(tmpParam, "base_link_joint");
EXPECT_TRUE(sp->seenAll());
EXPECT_EQ(0.5, sp->getParams()[0]);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-02-18 00:15:06
|
Revision: 11282
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11282&view=rev
Author: tfoote
Date: 2009-02-18 00:15:00 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
making tf API consistent by using ros::Duration #847
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp
pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp
pkg/trunk/mapping/door_handle_detector/test/door_checkerboard_detector.cpp
pkg/trunk/mapping/door_handle_detector/test/test_door_detection.cpp
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/slam_gmapping/src/tftest.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
pkg/trunk/prcore/tf/include/tf/tf.h
pkg/trunk/prcore/tf/include/tf/transform_listener.h
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/visualization/rviz/src/rviz/visualization_manager.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -160,7 +160,7 @@
ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
HeadPanTiltControllerNode::HeadPanTiltControllerNode()
-: Controller(), node_(ros::Node::instance()), TF(*ros::Node::instance(),false, 10000000000ULL)
+: Controller(), node_(ros::Node::instance()), TF(*ros::Node::instance(),false, ros::Duration(10))
{
c_ = new HeadPanTiltController();
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -187,7 +187,7 @@
ROS_REGISTER_CONTROLLER(HeadServoingControllerNode)
HeadServoingControllerNode::HeadServoingControllerNode()
-: Controller(), node_(ros::Node::instance()), TF(*ros::Node::instance(),false, 10000000000ULL)
+: Controller(), node_(ros::Node::instance()), TF(*ros::Node::instance(),false, ros::Duration(10))
{
c_ = new HeadServoingController();
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -313,7 +313,7 @@
PlugControllerNode::PlugControllerNode()
-: node_(ros::Node::instance()), loop_count_(0), TF(*ros::Node::instance(),false, 10000000000ULL)
+: node_(ros::Node::instance()), loop_count_(0), TF(*ros::Node::instance(),false,ros::Duration(10.0))
{
current_frame_publisher_=NULL;
}
Modified: pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp
===================================================================
--- pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/deprecated/ransac_ground_plane_extraction/src/ransac_ground_plane_extraction_node.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -37,7 +37,7 @@
using namespace ransac_ground_plane_extraction;
RansacGroundPlaneExtractionNode::RansacGroundPlaneExtractionNode(std::string node_name):ros::Node(node_name),obstacle_cloud_(NULL),publish_obstacle_cloud_(false),
- tf_(*this, true, 10000000000ULL) // cache for 10 sec, no extrapolation
+ tf_(*this, true, ros::Duration(10)) // cache for 10 sec, no extrapolation
{
std::string publish_obstacle_cloud;
Modified: pkg/trunk/deprecated/robot_model/include/robot_model/knode.h
===================================================================
--- pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/deprecated/robot_model/include/robot_model/knode.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -94,7 +94,7 @@
planning_models::KinematicModel::StateParams* getRobotStateSimple() { return m_robotStateSimple; }
- NodeRobotModel(ros::Node *node, const std::string &robot_model_name) : m_tf(*node, true, 1000000000ULL)
+ NodeRobotModel(ros::Node *node, const std::string &robot_model_name) : m_tf(*node, true, ros::Duration(1))
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
m_urdf = NULL;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/include/point_cloud_assembler/base_assembler_srv.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -128,8 +128,7 @@
param("~tf_cache_time_secs", tf_cache_time_secs, 10.0) ;
if (tf_cache_time_secs < 0)
ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ;
- unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL ;
- tf_ = new tf::TransformListener(*this, true, tf_cache_time) ;
+ tf_ = new tf::TransformListener(*this, true, ros::Duration(tf_cache_time_secs)) ;
ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ;
// ***** Set max_scans *****
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/test/groundtruthtransform.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -13,7 +13,7 @@
robot_msgs::Point32 msg;
tf::TransformListener tf;
- GroundTruthTransform() : ros::Node("GroundTruthTransform"), tf(*this, true, (uint64_t)10000000000ULL) {
+ GroundTruthTransform() : ros::Node("GroundTruthTransform"), tf(*this, true, ros::Duration(10)) {
advertise<robot_msgs::Point32>("groundtruthposition", 1);
}
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -53,7 +53,7 @@
MoveBase::MoveBase()
: HighlevelController<robot_msgs::Planner2DState, robot_msgs::Planner2DGoal>("move_base", "state", "goal"),
- tf_(*this, true, 10000000000ULL), // cache for 10 sec, no extrapolation
+ tf_(*this, true, ros::Duration(10)), // cache for 10 sec, no extrapolation
controller_(NULL),
costMap_(NULL),
global_map_accessor_(NULL),
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_end_effector.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -144,7 +144,7 @@
MoveEndEffector::MoveEndEffector(const std::string& nodeName, const std::string& stateTopic, const std::string& goalTopic,
const std::string& armPosTopic, const std::string& _armCmdTopic, const std::string& _kinematicModel)
: HighlevelController<pr2_msgs::MoveEndEffectorState, pr2_msgs::MoveEndEffectorGoal>(nodeName, stateTopic, goalTopic),
- armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0), tf_(*this, true, 10000000000ULL) {
+ armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0), tf_(*this, true, ros::Duration(10)) {
// Subscribe to arm configuration messages
subscribe(armPosTopic, mechanismState, &MoveEndEffector::handleArmConfigurationCallback, QUEUE_MAX());
Modified: pkg/trunk/mapping/door_handle_detector/test/door_checkerboard_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/test/door_checkerboard_detector.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/mapping/door_handle_detector/test/door_checkerboard_detector.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -55,7 +55,7 @@
tf::TransformListener tf_; /**< Used to do transforms */
robot_msgs::Door door_msg_;
- DoorCheckerboardDetectorNode(std::string node_name):ros::Node(node_name),tf_(*this, false, 10000000000ULL)
+ DoorCheckerboardDetectorNode(std::string node_name):ros::Node(node_name),tf_(*this, false, ros::Duration(10))
{
this->param<std::string>("door_checkerboard_detector/listen_topic",listen_topic_,"/checkerdetector/ObjectDetection");
this->param<std::string>("door_checkerboard_detector/publish_topic",publish_topic_,"door_location");
Modified: pkg/trunk/mapping/door_handle_detector/test/test_door_detection.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/test/test_door_detection.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/mapping/door_handle_detector/test/test_door_detection.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -61,7 +61,7 @@
robot_msgs::Door door_msg_from_detector_;
robot_msgs::Door door_msg_to_detector_;
- TestDoorDetectionNode(std::string node_name):ros::Node(node_name),tf_(*this, false, 10000000000ULL)
+ TestDoorDetectionNode(std::string node_name):ros::Node(node_name),tf_(*this, false, ros::Duration(10))
{
this->param<std::string>("test_door_detection_node/joy_topic",joy_topic_,"annotation_msg");
this->param<std::string>("test_door_detection_node/frame_id",frame_id_,"base_link");
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -95,7 +95,7 @@
planning_models::KinematicModel* getKModel(void) { return m_kmodel; }
planning_models::KinematicModel::StateParams* getRobotState() { return m_robotState; }
- KinematicStateMonitor(ros::Node *node) : m_tf(*node, true, 1000000000ULL)
+ KinematicStateMonitor(ros::Node *node) : m_tf(*node, true, ros::Duration(1))
{
m_tf.setExtrapolationLimit(ros::Duration().fromSec(10));
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -56,7 +56,7 @@
/// @todo Disable extrapolation, and implement scan-buffering. This is
/// not urgent for a robot like the PR2, where odometry is being
/// published several times faster than laser scans.
- tf_ = new tf::TransformListener(*node_, true, 10000000000ULL);
+ tf_ = new tf::TransformListener(*node_, true, ros::Duration(10));
tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2));
ROS_ASSERT(tf_);
Modified: pkg/trunk/nav/slam_gmapping/src/tftest.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/nav/slam_gmapping/src/tftest.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -9,7 +9,7 @@
{
node_ = new ros::Node("test");
tf_ = new tf::TransformListener(*node_, true,
- 10000000000ULL);
+ ros::Duration(10));
tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2));
node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1);
Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -40,7 +40,7 @@
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, (uint64_t)10000000000ULL),
+ 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,
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2009-02-18 00:15:00 UTC (rev 11282)
@@ -289,7 +289,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true, 10000000000ULL) // cache for 10 sec, no extrapolation
+ tf(*this, true, ros::Duration(10)) // cache for 10 sec, no extrapolation
{
// Initialize global pose. Will be set in control loop based on actual data.
///\todo does this need to be initialized? global_pose.setIdentity();
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -98,8 +98,7 @@
pnode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
if (tf_cache_time_secs < 0)
RAVELOG_ERRORA("ROSSensorSystem: Parameter tf_cache_time_secs<0 (%f)\n", tf_cache_time_secs);
- unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
- _tf.reset(new tf::TransformListener(*pnode, true, tf_cache_time));
+ _tf.reset(new tf::TransformListener(*pnode, true, ros::Duration(tf_cache_time_secs)));
RAVELOG_INFOA("ROSSensorSystem: TF Cache Time: %f Seconds\n", tf_cache_time_secs);
// **** Set TF Extrapolation Limit ****
Modified: pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/openrave_planning/orrosplanning/src/objecttransformsystem.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -106,8 +106,7 @@
pnode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
if (tf_cache_time_secs < 0)
RAVELOG_ERRORA("ROSSensorSystem: Parameter tf_cache_time_secs<0 (%f)\n", tf_cache_time_secs);
- unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
- _tf.reset(new tf::TransformListener(*pnode, true, tf_cache_time));
+ _tf.reset(new tf::TransformListener(*pnode, true, ros::Duration(tf_cache_time_secs)));
RAVELOG_INFOA("ROSSensorSystem: TF Cache Time: %f Seconds\n", tf_cache_time_secs);
// **** Set TF Extrapolation Limit ****
Modified: pkg/trunk/prcore/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/tf.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/prcore/tf/include/tf/tf.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -83,7 +83,7 @@
public:
/************* Constants ***********************/
static const unsigned int MAX_GRAPH_DEPTH = 100UL; //!< The maximum number of time to recurse before assuming the tree has a loop.
- static const int64_t DEFAULT_CACHE_TIME = 10ULL * 1000000000ULL; //!< The default amount of time to cache data
+ static const int64_t DEFAULT_CACHE_TIME = 10.0; //!< The default amount of time to cache data in seconds
static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL; //!< The default amount of time to extrapolate
Modified: pkg/trunk/prcore/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/prcore/tf/include/tf/transform_listener.h 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/prcore/tf/include/tf/transform_listener.h 2009-02-18 00:15:00 UTC (rev 11282)
@@ -55,9 +55,9 @@
* \param max_cache_time How long to store transform information */
TransformListener(ros::Node & rosnode,
bool interpolating = true,
- int64_t max_cache_time = DEFAULT_CACHE_TIME):
+ ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME)):
Transformer(interpolating,
- ros::Duration().fromNSec(max_cache_time)),
+ max_cache_time),
node_(rosnode)
{
// printf("Constructed rosTF\n");
Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -102,8 +102,7 @@
s_pmasternode->param("~tf_cache_time_secs", tf_cache_time_secs, 10.0);
if (tf_cache_time_secs < 0)
ROS_ERROR("RobotLinksFilter: Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs);
- unsigned long long tf_cache_time = tf_cache_time_secs*1000000000ULL;
- _tf.reset(new tf::TransformListener(*s_pmasternode, true, tf_cache_time));
+ _tf.reset(new tf::TransformListener(*s_pmasternode, true, ros::Duration(tf_cache_time_secs)));
ROS_INFO("RobotLinksFilter: TF Cache Time: %f Seconds", tf_cache_time_secs);
// **** Set TF Extrapolation Limit ****
Modified: pkg/trunk/visualization/rviz/src/rviz/visualization_manager.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/visualization_manager.cpp 2009-02-18 00:06:27 UTC (rev 11281)
+++ pkg/trunk/visualization/rviz/src/rviz/visualization_manager.cpp 2009-02-18 00:15:00 UTC (rev 11282)
@@ -93,7 +93,7 @@
}
ROS_ASSERT( ros_node_ );
- tf_ = new tf::TransformListener( *ros_node_, true, (uint64_t)10000000000ULL);
+ tf_ = new tf::TransformListener( *ros_node_, true, ros::Duration(10));
scene_manager_ = ogre_root_->createSceneManager( Ogre::ST_GENERIC );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-02-18 23:32:06
|
Revision: 11347
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11347&view=rev
Author: vijaypradeep
Date: 2009-02-18 22:14:18 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
Adding phasespace body defs, and fixing more teleop launch files.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2_phase_space/phase_space_defs/tongs_1_ruler.rb
Added Paths:
-----------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/master_teleop.launch
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/phase_space_defs/tongs_1_ruler.rb
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/phase_space_defs/tongs_1_ruler.rb 2009-02-18 22:01:54 UTC (rev 11346)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/phase_space_defs/tongs_1_ruler.rb 2009-02-18 22:14:18 UTC (rev 11347)
@@ -5,4 +5,4 @@
4, 214.00 28.00 -28.0
5, 221.00 0.00 -3.00
6, 225.00 -10.00 -1.00
-7, 198.00 -4.00 -28.0
+7, 198.00 -15.00 -28.0
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs13.rb 2009-02-18 22:14:18 UTC (rev 11347)
@@ -0,0 +1,16 @@
+0, -105.34 0.00 0.00
+1, -58.81 -2.79 26.35
+2, -61.41 -21.99 30.36
+3, -68.16 -26.36 15.17
+4, -3.29 -14.90 42.76
+5, -18.97 -39.18 56.78
+6, -22.29 -43.86 37.44
+7, -34.19 -44.18 20.76
+8, -80.45 5.23 -40.77
+9, -33.99 -19.06 -41.70
+10, -29.03 -12.42 -37.33
+11, -36.04 -5.23 -23.89
+12, -5.00 -56.40 -7.10
+13, 7.77 -28.04 -15.04
+14, 17.65 -24.83 -10.34
+15, 1.27 -2.26 -2.96
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/bodies/tongs46.rb 2009-02-18 22:14:18 UTC (rev 11347)
@@ -0,0 +1,16 @@
+16, -118.83 0.41 -0.51
+17, -107.87 40.54 8.36
+18, -59.95 -16.11 -19.74
+19, -56.06 24.66 -42.82
+20, -49.93 44.77 -7.53
+21, -22.54 -22.55 -45.98
+22, -1.90 13.67 -40.24
+23, -8.71 48.06 -45.74
+24, -104.74 32.04 46.83
+25, -112.26 -15.36 40.75
+26, -46.58 34.65 30.32
+27, -31.37 -9.49 40.71
+28, -51.77 -32.64 16.52
+29, -0.80 20.22 5.61
+30, 19.16 -18.43 7.52
+31, -11.95 -37.55 0.63
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/master_teleop.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/master_teleop.launch (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_teleop/master_teleop.launch 2009-02-18 22:14:18 UTC (rev 11347)
@@ -0,0 +1,8 @@
+<launch>
+
+ <!-- Base Pedals -->
+ <include file="$(find teleop_base_pedals)/teleop_base_pedals.launch" />
+
+ <!-- Phase Space -->
+ <node pkg="phase_space" type="phase_space_node" output="log" />
+</launch>
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-18 23:33:21
|
Revision: 11348
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11348&view=rev
Author: isucan
Date: 2009-02-18 22:16:43 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
small optimization for planning models forward kinematics: calling computeTransform() with the same argument twice in a row does not redo computation; projection definition for ompl was moved in base/ ; projections are better organized in kinematic_lanning added a projection for the end effector position
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/EST.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/est/src/EST.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/LBKPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/LBKPIECE1.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/SBL.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/sbl/src/SBL.cpp
pkg/trunk/motion_planning/ompl/code/tests/samplingbased/kinematic/2dmap/2dmap.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
pkg/trunk/world_models/robot_models/planning_models/test/test_kinematic.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h
Removed Paths:
-------------
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -125,7 +125,7 @@
void update(ompl::SpaceInformationKinematic::StateKinematic_t state) const
{
- m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->kmodel->computeTransformsGroup(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
Added: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPDistanceEvaluators.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,48 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef KINEMATIC_PLANNING_RKP_DISTANCE_EVALUATORS
+#define KINEMATIC_PLANNING_RKP_DISTANCE_EVALUATORS
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include "kinematic_planning/RKPModelBase.h"
+
+namespace kinematic_planning
+{
+ // no additional distance evaluators at this point
+}
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPESTSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::EST_t est = new ompl::EST(si);
mp = est;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], est->getRange());
@@ -79,44 +76,11 @@
ROS_INFO("Range is set to %g", range);
}
- if (options.find("projection") != options.end())
- {
- std::string proj = options["projection"];
- std::vector<unsigned int> projection;
- std::stringstream ss(proj);
- while (ss.good())
- {
- unsigned int comp;
- ss >> comp;
- projection.push_back(comp);
- }
-
- est->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ est->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (est->getProjectionEvaluator() == NULL)
{
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- est->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
- ROS_WARN("Adding EST failed: need to set both 'projection' and 'celldim' for %s", model->groupName.c_str());
+ ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
else
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPIKSBLSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,53 +69,17 @@
ompl::IKSBL* sbl = new ompl::IKSBL(si);
mp = sbl;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], sbl->getRange());
sbl->setRange(range);
ROS_INFO("Range is set to %g", range);
}
+
+ sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("projection") != options.end())
+ if (sbl->getProjectionEvaluator() == NULL)
{
- std::string proj = options["projection"];
- std::vector<unsigned int> projection;
- std::stringstream ss(proj);
- while (ss.good())
- {
- unsigned int comp;
- ss >> comp;
- projection.push_back(comp);
- }
-
- sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
-
- if (options.find("celldim") != options.end())
- {
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- sbl->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPKPIECESetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::LBKPIECE1_t kpiece = new ompl::LBKPIECE1(si);
mp = kpiece;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], kpiece->getRange());
@@ -79,43 +76,10 @@
ROS_INFO("Range is set to %g", range);
}
- if (options.find("projection") != options.end())
- {
- std::string proj = options["projection"];
- std::vector<unsigned int> projection;
- std::stringstream ss(proj);
- while (ss.good())
- {
- unsigned int comp;
- ss >> comp;
- projection.push_back(comp);
- }
-
- kpiece->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ kpiece->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (kpiece->getProjectionEvaluator() == NULL)
{
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- kpiece->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPPlannerSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -40,12 +40,15 @@
#include "kinematic_planning/RKPModelBase.h"
#include "kinematic_planning/RKPStateValidator.h"
#include "kinematic_planning/RKPSpaceInformation.h"
+#include "kinematic_planning/RKPProjectionEvaluators.h"
+#include "kinematic_planning/RKPDistanceEvaluators.h"
#include <ompl/base/Planner.h>
#include <ompl/extension/samplingbased/kinematic/PathSmootherKinematic.h>
#include <ros/console.h>
#include <boost/lexical_cast.hpp>
+#include <boost/algorithm/string.hpp>
#include <cassert>
#include <vector>
@@ -89,6 +92,55 @@
sde["L2Square"] = new ompl::SpaceInformationKinematic::StateKinematicL2SquareDistanceEvaluator(si);
}
+ virtual ompl::ProjectionEvaluator_t getProjectionEvaluator(RKPModelBase *model, const std::map<std::string, std::string> &options) const
+ {
+ std::map<std::string, std::string>::const_iterator pit = options.find("projection");
+ std::map<std::string, std::string>::const_iterator cit = options.find("celldim");
+ ompl::ProjectionEvaluator_t pe = NULL;
+
+ if (pit != options.end() && cit != options.end())
+ {
+ std::string proj = pit->second;
+ boost::trim(proj);
+ std::string celldim = cit->second;
+ boost::trim(celldim);
+
+ if (proj.substr(0, 4) == "link")
+ {
+ std::string linkName = proj.substr(4);
+ boost::trim(linkName);
+ pe = new LinkPositionProjectionEvaluator(model, linkName);
+ }
+ else
+ {
+ std::vector<unsigned int> projection;
+ std::stringstream ss(proj);
+ while (ss.good())
+ {
+ unsigned int comp;
+ ss >> comp;
+ projection.push_back(comp);
+ }
+ pe = new ompl::OrthogonalProjectionEvaluator(projection);
+ }
+
+ std::vector<double> cdim;
+ std::stringstream ss(celldim);
+ while (ss.good())
+ {
+ double comp;
+ ss >> comp;
+ cdim.push_back(comp);
+ }
+
+ pe->setCellDimensions(cdim);
+ ROS_INFO("Projection is set to %s", proj.c_str());
+ ROS_INFO("Cell dimensions set to %s", celldim.c_str());
+ }
+
+ return pe;
+ }
+
virtual void preSetup(RKPModelBase *model, std::map<std::string, std::string> &options)
{
ROS_INFO("Adding %s instance for motion planning: %s", name.c_str(), model->groupName.c_str());
Added: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPProjectionEvaluators.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,85 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#ifndef KINEMATIC_PLANNING_RKP_PROJECTION_EVALUATORS
+#define KINEMATIC_PLANNING_RKP_PROJECTION_EVALUATORS
+
+#include <ompl/extension/samplingbased/kinematic/ProjectionEvaluatorKinematic.h>
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include "kinematic_planning/RKPModelBase.h"
+
+namespace kinematic_planning
+{
+
+ class LinkPositionProjectionEvaluator : public ompl::ProjectionEvaluator
+ {
+ public:
+
+ LinkPositionProjectionEvaluator(RKPModelBase *model, const std::string &linkName) : ompl::ProjectionEvaluator()
+ {
+ m_model = model;
+ m_link = m_model->kmodel->getLink(linkName);
+ }
+
+ /** Return the dimension of the projection defined by this evaluator */
+ virtual unsigned int getDimension(void) const
+ {
+ return 3;
+ }
+
+ /** Compute the projection as an array of double values */
+ virtual void operator()(const ompl::SpaceInformation::State *state, double *projection) const
+ {
+ const ompl::SpaceInformationKinematic::StateKinematic *kstate = static_cast<const ompl::SpaceInformationKinematic::StateKinematic*>(state);
+ m_model->lock.lock();
+ m_model->kmodel->computeTransformsGroup(kstate->values, m_model->groupID);
+ const btVector3 &origin = m_link->globalTrans.getOrigin();
+ projection[0] = origin.x();
+ projection[1] = origin.y();
+ projection[2] = origin.z();
+ m_model->lock.unlock();
+ }
+
+ protected:
+
+ RKPModelBase *m_model;
+ planning_models::KinematicModel::Link *m_link;
+
+ };
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPSBLSetup.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -69,9 +69,6 @@
ompl::SBL_t sbl = new ompl::SBL(si);
mp = sbl;
- bool setDim = false;
- bool setProj = false;
-
if (options.find("range") != options.end())
{
double range = parseDouble(options["range"], sbl->getRange());
@@ -79,43 +76,10 @@
ROS_INFO("Range is set to %g", range);
}
- if (options.find("projection") != options.end())
- {
- std::string proj = options["projection"];
- std::vector<unsigned int> projection;
- std::stringstream ss(proj);
- while (ss.good())
- {
- unsigned int comp;
- ss >> comp;
- projection.push_back(comp);
- }
-
- sbl->setProjectionEvaluator(new ompl::OrthogonalProjectionEvaluator(projection));
-
- ROS_INFO("Projection is set to %s", proj.c_str());
- setProj = true;
- }
+ sbl->setProjectionEvaluator(getProjectionEvaluator(model, options));
- if (options.find("celldim") != options.end())
+ if (sbl->getProjectionEvaluator() == NULL)
{
- std::string celldim = options["celldim"];
- std::vector<double> cdim;
- std::stringstream ss(celldim);
- while (ss.good())
- {
- double comp;
- ss >> comp;
- cdim.push_back(comp);
- }
-
- sbl->setCellDimensions(cdim);
- setDim = true;
- ROS_INFO("Cell dimensions set to %s", celldim.c_str());
- }
-
- if (!setDim || !setProj)
- {
ROS_WARN("Adding %s failed: need to set both 'projection' and 'celldim' for %s", name.c_str(), model->groupName.c_str());
return false;
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPStateValidator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -57,7 +57,7 @@
virtual bool operator()(const ompl::SpaceInformation::State_t state) const
{
m_model->lock.lock();
- m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->kmodel->computeTransformsGroup(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-02-18 22:16:43 UTC (rev 11348)
@@ -225,23 +225,31 @@
/** Free the memory */
virtual ~KinematicPlanning(void)
{
+ ROS_INFO("A");
+
+
stopReplanning();
+ ROS_INFO("B");
stopPublishingStatus();
+ ROS_INFO("C");
for (std::map<std::string, RKPModel*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
delete i->second;
+ ROS_INFO("D");
}
- bool isSafeToPlan(void)
+ bool isSafeToPlan(bool report)
{
if (!isMapUpdated(m_intervalCollisionMap))
{
- ROS_WARN("Planning is not safe: map is not up to date");
+ if (report)
+ ROS_WARN("Planning is not safe: map is not up to date");
return false;
}
if (!isStateUpdated(m_intervalKinematicState))
{
- ROS_WARN("Planning is not safe: kinematic state is not up to date");
+ if (report)
+ ROS_WARN("Planning is not safe: kinematic state is not up to date");
return false;
}
@@ -295,8 +303,11 @@
void startPublishingStatus(void)
{
- m_publishStatus = true;
- m_statusThread = new boost::thread(boost::bind(&KinematicPlanning::publishStatus, this));
+ if (loadedRobot())
+ {
+ m_publishStatus = true;
+ m_statusThread = new boost::thread(boost::bind(&KinematicPlanning::publishStatus, this));
+ }
}
void stopPublishingStatus(void)
@@ -358,7 +369,7 @@
bool result = false;
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
res.value.id = -1;
res.value.done = trivial ? 1 : 0;
@@ -379,7 +390,7 @@
bool result = false;
- res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ res.value.unsafe = isSafeToPlan(true) ? 0 : 1;
result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
res.value.id = -1;
@@ -455,7 +466,7 @@
{
ros::Time nextTime = ros::Time::now() + duration;
bool wait = true;
- while (wait && ros::Time::now() < nextTime)
+ while (m_publishStatus && wait && ros::Time::now() < nextTime)
{
delta.sleep();
if (m_statusLock.try_lock())
@@ -465,7 +476,7 @@
m_statusLock.unlock();
}
}
-
+
bool issueStop = false;
bool replan_inactive = m_continueReplanningLock.try_lock();
@@ -495,7 +506,7 @@
// the sent safety value is the one before the motion
// planning started
if (m_currentPlanStatus.path.states.empty())
- m_currentPlanStatus.unsafe = isSafeToPlan() ? 0 : 1;
+ m_currentPlanStatus.unsafe = isSafeToPlan(false) ? 0 : 1;
publish("kinematic_planning_status", m_currentPlanStatus);
@@ -579,7 +590,7 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
- bool safe = isSafeToPlan();
+ bool safe = isSafeToPlan(true);
currentState(m_currentPlanToStateRequest.start_state);
m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
@@ -626,7 +637,7 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
- bool safe = isSafeToPlan();
+ bool safe = isSafeToPlan(true);
currentState(m_currentPlanToPositionRequest.start_state);
m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
Added: pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/ProjectionEvaluator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -0,0 +1,85 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#ifndef OMPL_BASE_PROJECTION_EVALUATOR_
+#define OMPL_BASE_PROJECTION_EVALUATOR_
+
+#include "ompl/base/SpaceInformation.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(ProjectionEvaluator);
+
+ /** Abstract definition for a class computing projections */
+ class ProjectionEvaluator
+ {
+ public:
+ /** Destructor */
+ virtual ~ProjectionEvaluator(void)
+ {
+ }
+
+ /** Define the dimension (each component) of a grid cell. The
+ number of dimensions set here must be the same as the
+ dimension of the projection computed by the projection
+ evaluator. */
+ void setCellDimensions(const std::vector<double> &cellDimensions)
+ {
+ m_cellDimensions = cellDimensions;
+ }
+
+ void getCellDimensions(std::vector<double> &cellDimensions) const
+ {
+ cellDimensions = m_cellDimensions;
+ }
+
+ /** Return the dimension of the projection defined by this evaluator */
+ virtual unsigned int getDimension(void) const = 0;
+
+ /** Compute the projection as an array of double values */
+ virtual void operator()(const SpaceInformation::State *state, double *projection) const = 0;
+
+ protected:
+
+ std::vector<double> m_cellDimensions;
+
+ };
+
+}
+
+#endif
Deleted: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2009-02-18 22:14:18 UTC (rev 11347)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/ProjectionEvaluator.h 2009-02-18 22:16:43 UTC (rev 11348)
@@ -1,96 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are p...
[truncated message content] |
|
From: <tf...@us...> - 2009-02-18 23:35:58
|
Revision: 11356
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11356&view=rev
Author: tfoote
Date: 2009-02-18 23:35:50 +0000 (Wed, 18 Feb 2009)
Log Message:
-----------
cleaning up angles package trying to code review. lots of tests added, and exposed some problems
Modified Paths:
--------------
pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp
pkg/trunk/prcore/angles/CMakeLists.txt
pkg/trunk/prcore/angles/include/angles/angles.h
pkg/trunk/prcore/angles/test/utest.cpp
Modified: pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp
===================================================================
--- pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp 2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/deprecated/libTF/test/pose3d_unittest.cpp 2009-02-18 23:35:50 UTC (rev 11356)
@@ -4,6 +4,21 @@
#include <sys/time.h>
#include <cstdlib>
+ /*!
+ * \brief modNPiBy2
+ *
+ * Returns the angle between -M_PI/2 to M_PI/2
+ */
+static inline double angles::modNPiBy2(double angle)
+ {
+ if (angle < -M_PI/2)
+ angle += M_PI;
+ if(angle > M_PI/2)
+ angle -= M_PI;
+ return angle;
+ }
+
+
//Seed random number generator with current microseond count
void seed_rand(){
timeval temp_time_struct;
Modified: pkg/trunk/prcore/angles/CMakeLists.txt
===================================================================
--- pkg/trunk/prcore/angles/CMakeLists.txt 2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/CMakeLists.txt 2009-02-18 23:35:50 UTC (rev 11356)
@@ -4,6 +4,4 @@
# Unit tests
rospack_add_gtest(utest test/utest.cpp)
-set_target_properties(utest PROPERTIES
- RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
target_link_libraries(utest)
Modified: pkg/trunk/prcore/angles/include/angles/angles.h
===================================================================
--- pkg/trunk/prcore/angles/include/angles/angles.h 2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/include/angles/angles.h 2009-02-18 23:35:50 UTC (rev 11356)
@@ -263,19 +263,7 @@
- /*!
- * \brief modNPiBy2
- *
- * Returns the angle between -M_PI/2 to M_PI/2
- */
- static inline double modNPiBy2(double angle)
- {
- if (angle < -M_PI/2)
- angle += M_PI;
- if(angle > M_PI/2)
- angle -= M_PI;
- return angle;
- }
+
}
#endif
Modified: pkg/trunk/prcore/angles/test/utest.cpp
===================================================================
--- pkg/trunk/prcore/angles/test/utest.cpp 2009-02-18 23:10:06 UTC (rev 11355)
+++ pkg/trunk/prcore/angles/test/utest.cpp 2009-02-18 23:35:50 UTC (rev 11356)
@@ -1,6 +1,8 @@
#include "angles/angles.h"
#include <gtest/gtest.h>
+using namespace angles;
+
TEST(Angles, shortestDistanceWithLimits){
double shortest_angle;
bool result = angles::shortest_angular_distance_with_limits(-0.5, 0.5,-0.25,0.25,shortest_angle);
@@ -55,6 +57,158 @@
}
+TEST(Angles, from_degrees)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(0, from_degrees(0), epsilon);
+ EXPECT_NEAR(M_PI/2, from_degrees(90), epsilon);
+ EXPECT_NEAR(M_PI, from_degrees(180), epsilon);
+ EXPECT_NEAR(M_PI*3/2, from_degrees(270), epsilon);
+ EXPECT_NEAR(2*M_PI, from_degrees(360), epsilon);
+ EXPECT_NEAR(M_PI/3, from_degrees(60), epsilon);
+ EXPECT_NEAR(M_PI*2/3, from_degrees(120), epsilon);
+ EXPECT_NEAR(M_PI/4, from_degrees(45), epsilon);
+ EXPECT_NEAR(M_PI*3/4, from_degrees(135), epsilon);
+ EXPECT_NEAR(M_PI/6, from_degrees(30), epsilon);
+
+}
+
+TEST(Angles, to_degrees)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(to_degrees(0), 0, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI/2), 90, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI), 180, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI*3/2), 270, epsilon);
+ EXPECT_NEAR(to_degrees(2*M_PI), 360, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI/3), 60, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI*2/3), 120, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI/4), 45, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI*3/4), 135, epsilon);
+ EXPECT_NEAR(to_degrees(M_PI/6), 30, epsilon);
+}
+
+TEST(Angles, normalize_angle_positive)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(-0), epsilon);
+ EXPECT_NEAR(3*M_PI/2, normalize_angle_positive(-M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle_positive(-M_PI), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
+ EXPECT_NEAR(0, normalize_angle_positive(-4*M_PI/2), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle_positive(0), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(5*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(9*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle_positive(-3*M_PI/2), epsilon);
+
+}
+
+
+TEST(Angles, normalize_angle)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(0, normalize_angle(0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(-0), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-2*M_PI), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-3*M_PI), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-4*M_PI), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(-0), epsilon);
+ EXPECT_NEAR(-M_PI/2, normalize_angle(-M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI, normalize_angle(-M_PI), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
+ EXPECT_NEAR(0, normalize_angle(-4*M_PI/2), epsilon);
+
+ EXPECT_NEAR(0, normalize_angle(0), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(5*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(9*M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, normalize_angle(-3*M_PI/2), epsilon);
+
+}
+
+TEST(Angles, shortest_angular_distance)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(0, M_PI/2), epsilon);
+ EXPECT_NEAR(-M_PI/2, shortest_angular_distance(0, -M_PI/2), epsilon);
+ EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI/2, 0), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(-M_PI/2, 0), epsilon);
+
+ EXPECT_NEAR(-M_PI/2, shortest_angular_distance(M_PI, M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI, -M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(M_PI/2, M_PI), epsilon);
+ EXPECT_NEAR(-M_PI/2, shortest_angular_distance(-M_PI/2, M_PI), epsilon);
+
+ EXPECT_NEAR(-M_PI/2, shortest_angular_distance(5*M_PI, M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(7*M_PI, -M_PI/2), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(9*M_PI/2, M_PI), epsilon);
+ EXPECT_NEAR(M_PI/2, shortest_angular_distance(-3*M_PI/2, M_PI), epsilon);
+
+}
+
+TEST(Angles, add_mod_2Pi)
+{
+ double epsilon = 1e-9;
+ EXPECT_NEAR(add_mod_2Pi(0), -2*M_PI, epsilon); //Should this be?
+ EXPECT_NEAR(add_mod_2Pi(2*M_PI), 0, epsilon);
+ EXPECT_NEAR(add_mod_2Pi(-2*M_PI), 0, epsilon);
+ EXPECT_NEAR(add_mod_2Pi(M_PI/2), -3*M_PI/2, epsilon);
+ EXPECT_NEAR(add_mod_2Pi(M_PI), -M_PI, epsilon);
+ EXPECT_NEAR(add_mod_2Pi(-M_PI), M_PI, epsilon);
+ EXPECT_NEAR(add_mod_2Pi(-M_PI/2), 3*M_PI/2, epsilon);
+
+
+
+}
+
+/** \todo find_min_max_delta is always returning false commented until fixed
+TEST(Angles, find_min_max_delta)
+{
+ double epsilon = 1e-9;
+ double min_delta, max_delta;
+ EXPECT_TRUE(find_min_max_delta( 0, -M_PI, M_PI, min_delta, max_delta));
+ EXPECT_NEAR(min_delta, -M_PI, epsilon);
+ EXPECT_NEAR(max_delta, M_PI, epsilon);
+
+ EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, M_PI, min_delta, max_delta));
+ EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
+ EXPECT_NEAR(max_delta, M_PI/2, epsilon);
+
+ EXPECT_TRUE(find_min_max_delta( M_PI/2, -M_PI, 3*M_PI, min_delta, max_delta));
+ EXPECT_NEAR(min_delta, -3*M_PI/2, epsilon);
+ EXPECT_NEAR(max_delta, M_PI/2, epsilon);
+
+ EXPECT_TRUE(find_min_max_delta( -M_PI/2, -M_PI, 3*M_PI, min_delta, max_delta));
+ EXPECT_NEAR(min_delta, -M_PI/2, epsilon);
+ EXPECT_NEAR(max_delta, 3*M_PI/2, epsilon);
+
+
+ EXPECT_FALSE(find_min_max_delta( 2*M_PI, -M_PI, M_PI, min_delta, max_delta));
+ EXPECT_FALSE(find_min_max_delta( -M_PI, -M_PI/2, M_PI/2, min_delta, max_delta));
+
+}
+*/
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-02-19 07:28:56
|
Revision: 11392
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11392&view=rev
Author: isucan
Date: 2009-02-19 07:28:48 +0000 (Thu, 19 Feb 2009)
Log Message:
-----------
separated hillclimbig and added kpiece as default planner
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/CMakeLists.txt
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
Added Paths:
-----------
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp
Modified: pkg/trunk/motion_planning/ompl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/CMakeLists.txt 2009-02-19 07:28:48 UTC (rev 11392)
@@ -3,13 +3,14 @@
rospack(ompl)
rospack_add_boost_directories()
include_directories(${PROJECT_SOURCE_DIR}/code)
-set(CMAKE_BUILD_TYPE Release)
+#set(CMAKE_BUILD_TYPE Release)
rospack_add_library(ompl code/ompl/base/util/src/time.cpp
code/ompl/base/util/src/output.cpp
code/ompl/base/util/src/random_utils.cpp
code/ompl/base/src/Planner.cpp
code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
code/ompl/extension/samplingbased/kinematic/src/PathSmootherKinematic.cpp
+ code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp
code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -67,7 +67,7 @@
}
/** Destructor */
- ~SpaceInformationKinematic(void)
+ virtual ~SpaceInformationKinematic(void)
{
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/GAIK.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -34,11 +34,11 @@
/* \author Ioan Sucan */
-#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_GAIK_
-#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_GAIK_
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_GAIK_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_GAIK_
-#include "ompl/base/Planner.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
namespace ompl
{
@@ -56,17 +56,17 @@
@par External documentation
*/
- class GAIK : public Planner
+ class GAIK
{
public:
- GAIK(SpaceInformation_t si) : Planner(si)
- {
- m_type = PLAN_TO_GOAL_REGION;
+ GAIK(SpaceInformationKinematic_t si) : m_hcik(si)
+ {
+ m_si = si;
m_rho = 0.04;
m_poolSize = 80;
m_poolExpansion = 100;
- m_maxImproveSteps = 3;
+ m_hcik.setMaxImproveSteps(3);
m_checkValidity = true;
}
@@ -74,20 +74,16 @@
{
}
- virtual bool solve(double solveTime);
+ virtual bool solve(double solveTime, SpaceInformationKinematic::StateKinematic_t result);
- virtual void clear(void)
- {
- }
-
void setMaxImproveSteps(unsigned int maxSteps)
{
- m_maxImproveSteps = maxSteps;
+ m_hcik.setMaxImproveSteps(maxSteps);
}
unsigned int getMaxImproveSteps(void) const
{
- return m_maxImproveSteps;
+ return m_hcik.getMaxImproveSteps();
}
void setValidityCheck(bool valid)
@@ -135,8 +131,7 @@
protected:
- bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double *distance);
- bool tryToImproveAux(double add, SpaceInformationKinematic::StateKinematic_t state, double *distance);
+ bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double distance);
bool valid(SpaceInformationKinematic::StateKinematic_t state);
struct Individual
@@ -153,12 +148,16 @@
}
};
- unsigned int m_poolSize;
- unsigned int m_poolExpansion;
- unsigned int m_maxImproveSteps;
- bool m_checkValidity;
+ HCIK m_hcik;
+ SpaceInformationKinematic_t m_si;
+ unsigned int m_poolSize;
+ unsigned int m_poolExpansion;
+ unsigned int m_maxImproveSteps;
+ bool m_checkValidity;
- double m_rho;
+ double m_rho;
+
+ msg::Interface m_msg;
};
}
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -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 Ioan Sucan */
+
+#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_HCIK_
+#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_IK_HCIK_
+
+#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+
+namespace ompl
+{
+
+ /** Forward class declaration */
+ ForwardClassDeclaration(HCIK);
+
+ /**
+ @subsubsection HCIK Inverse Kinematics with Hill Climbing
+
+ @par Short description
+
+ HCIK does inverse kinematics with hill climbing, starting from a given state.
+
+ @par External documentation
+ */
+ class HCIK
+ {
+ public:
+
+ HCIK(SpaceInformationKinematic_t si)
+ {
+ m_si = si;
+ m_maxImproveSteps = 2;
+ }
+
+ virtual ~HCIK(void)
+ {
+ }
+
+ bool tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double add, double *distance = NULL) const;
+
+ void setMaxImproveSteps(unsigned int steps)
+ {
+ m_maxImproveSteps = steps;
+ }
+
+ unsigned int getMaxImproveSteps(void) const
+ {
+ return m_maxImproveSteps;
+ }
+
+ protected:
+
+ SpaceInformationKinematic_t m_si;
+ unsigned int m_maxImproveSteps;
+
+ };
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/IKPlanner.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -62,7 +62,7 @@
public:
IKPlanner(SpaceInformation_t si) : _P(si),
- m_gaik(si)
+ m_gaik(dynamic_cast<SpaceInformationKinematic_t>(si))
{
_P::m_type = _P::m_type | PLAN_TO_GOAL_REGION;
}
@@ -80,12 +80,6 @@
{
return m_gaik.getRange();
}
-
- virtual void setup(void)
- {
- m_gaik.setup();
- _P::setup();
- }
virtual bool solve(double solveTime)
{
@@ -128,7 +122,6 @@
bool solved = false;
unsigned int step = 0;
- m_gaik.clear();
while (!solved)
{
@@ -136,18 +129,14 @@
double time_left = (endTime - time_utils::Time::now()).toSeconds();
if (time_left <= 0.0)
break;
- if (m_gaik.solve(time_left * 0.6))
+ if (m_gaik.solve(time_left * 0.5, stateGoal->state))
{
- SpaceInformationKinematic::PathKinematic_t foundPath = static_cast<SpaceInformationKinematic::PathKinematic_t>(goal_r->getSolutionPath());
- assert(foundPath && foundPath->states.size() == 1);
-
/* change goal to a state one */
si->forgetGoal();
si->setGoal(stateGoal);
- si->copyState(stateGoal->state, foundPath->states[0]);
/* run _P on the new goal */
- clear();
+ _P::clear();
time_left = (endTime - time_utils::Time::now()).toSeconds();
_P::m_msg.inform("IKPlanner: Using GAIK goal state for the planner (step %u, %g seconds remaining)", step, time_left);
solved = _P::solve(time_left);
@@ -159,9 +148,12 @@
/* copy solution to actual goal instance */
if (solved)
{
- if (goal_r->isApproximate())
+ double dist = -1.0;
+ bool approx = !goal_r->isSatisfied(stateGoal->state, &dist);
+ if (approx)
_P::m_msg.warn("IKPlanner: Found approximate solution");
- goal_r->setSolutionPath(stateGoal->getSolutionPath(), goal_r->isApproximate());
+ goal_r->setSolutionPath(stateGoal->getSolutionPath(), approx);
+ goal_r->setDifference(dist);
stateGoal->forgetSolutionPath();
}
else
@@ -172,14 +164,7 @@
delete stateGoal;
return solved;
}
-
-
- virtual void clear(void)
- {
- m_gaik.clear();
- _P::clear();
- }
-
+
protected:
GAIK m_gaik;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/GAIK.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -39,14 +39,13 @@
bool ompl::GAIK::valid(SpaceInformationKinematic::StateKinematic_t state)
{
- return m_checkValidity ? m_si->isValid(static_cast<SpaceInformationKinematic::StateKinematic_t>(state)) : true;
+ return m_checkValidity ? m_si->isValid(static_cast<SpaceInformation::State_t>(state)) : true;
}
-bool ompl::GAIK::solve(double solveTime)
+bool ompl::GAIK::solve(double solveTime, SpaceInformationKinematic::StateKinematic_t result)
{
- SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
- SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
- unsigned int dim = si->getStateDimension();
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(m_si->getGoal());
+ unsigned int dim = m_si->getStateDimension();
if (!goal_r)
{
@@ -71,7 +70,7 @@
for (unsigned int i = 0 ; i < maxPoolSize ; ++i)
{
pool[i].state = new SpaceInformationKinematic::StateKinematic(dim);
- si->sample(pool[i].state);
+ m_si->sample(pool[i].state);
if (goal_r->isSatisfied(pool[i].state, &(pool[i].distance)))
{
if (valid(pool[i].state))
@@ -86,7 +85,7 @@
std::vector<double> range(dim);
for (unsigned int i = 0 ; i < dim ; ++i)
- range[i] = m_rho * (si->getStateComponent(i).maxValue - si->getStateComponent(i).minValue);
+ range[i] = m_rho * (m_si->getStateComponent(i).maxValue - m_si->getStateComponent(i).minValue);
while (!solved && time_utils::Time::now() < endTime)
{
@@ -95,7 +94,7 @@
for (unsigned int i = m_poolSize ; i < maxPoolSize ; ++i)
{
- si->sampleNear(pool[i].state, pool[i%m_poolSize].state, range);
+ m_si->sampleNear(pool[i].state, pool[i%m_poolSize].state, range);
if (goal_r->isSatisfied(pool[i].state, &(pool[i].distance)))
{
if (valid(pool[i].state))
@@ -113,24 +112,14 @@
if (solved)
{
// set the solution
- SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
- si->copyState(st, pool[solution].state);
- path->states.push_back(st);
- goal_r->setDifference(pool[solution].distance);
- goal_r->setSolutionPath(path);
+ m_si->copyState(result, pool[solution].state);
// try to improve the solution
- double dist = goal_r->getDifference();
- tryToImprove(st, &dist);
- goal_r->setDifference(dist);
+ tryToImprove(result, pool[solution].distance);
// if improving the state made it invalid, revert
- if (!valid(st))
- {
- si->copyState(st, pool[solution].state);
- goal_r->setDifference(pool[solution].distance);
- }
+ if (!valid(result))
+ m_si->copyState(result, pool[solution].state);
}
else
{
@@ -142,30 +131,15 @@
if (valid(pool[i].state))
{
// set the solution
- SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
- SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
- si->copyState(st, pool[i].state);
- path->states.push_back(st);
+ m_si->copyState(result, pool[i].state);
// try to improve the state
- double dist = pool[i].distance;
- tryToImprove(st, &dist);
+ tryToImprove(result, pool[i].distance);
- // if the improved state is still valid, we have a solution
- if (valid(st))
- {
- // the solution may no longer be just approximate, so we check
- bool approx = goal_r->isSatisfied(st, &dist);
- goal_r->setSolutionPath(path, approx);
- goal_r->setDifference(dist);
- }
- else
- {
- // if the improvement made the state no longer valid, revert to previous one
- si->copyState(st, pool[i].state);
- goal_r->setSolutionPath(path, true);
- goal_r->setDifference(pool[i].distance);
- }
+ // if the improvement made the state no longer valid, revert to previous one
+ if (!valid(result))
+ m_si->copyState(result, pool[i].state);
+ solved = true;
break;
}
}
@@ -174,101 +148,24 @@
for (unsigned int i = 0 ; i < maxPoolSize ; ++i)
delete pool[i].state;
- return goal_r->isAchieved();
+ return solved;
}
-bool ompl::GAIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double *distance)
+bool ompl::GAIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double distance)
{
- m_msg.inform("GAIK: Distance to goal before improvement: %g", *distance);
+ m_msg.inform("GAIK: Distance to goal before improvement: %g", distance);
time_utils::Time start = time_utils::Time::now();
- tryToImproveAux(0.1, state, distance);
- tryToImproveAux(0.05, state, distance);
- tryToImproveAux(0.01, state, distance);
- tryToImproveAux(0.005, state, distance);
- tryToImproveAux(0.001, state, distance);
- tryToImproveAux(0.0005, state, distance);
- tryToImproveAux(0.0001, state, distance);
- tryToImproveAux(0.00005, state, distance);
- tryToImproveAux(0.000025, state, distance);
- tryToImproveAux(0.00001, state, distance);
- tryToImproveAux(0.000005, state, distance);
+ m_hcik.tryToImprove(state, 0.1, &distance);
+ m_hcik.tryToImprove(state, 0.05, &distance);
+ m_hcik.tryToImprove(state, 0.01, &distance);
+ m_hcik.tryToImprove(state, 0.005, &distance);
+ m_hcik.tryToImprove(state, 0.001, &distance);
+ m_hcik.tryToImprove(state, 0.005, &distance);
+ m_hcik.tryToImprove(state, 0.0001, &distance);
+ m_hcik.tryToImprove(state, 0.00005, &distance);
+ m_hcik.tryToImprove(state, 0.000025, &distance);
+ m_hcik.tryToImprove(state, 0.000005, &distance);
m_msg.inform("GAIK: Improvement took %g seconds", (time_utils::Time::now() - start).toSeconds());
- m_msg.inform("GAIK: Distance to goal after improvement: %g", *distance);
+ m_msg.inform("GAIK: Distance to goal after improvement: %g", distance);
return true;
}
-
-bool ompl::GAIK::tryToImproveAux(double add, SpaceInformationKinematic::StateKinematic_t state, double *distance)
-{
- SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
- SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
- unsigned int dim = si->getStateDimension();
-
- bool wasSatisfied = goal_r->isSatisfied(state, distance);
- double bestDist = *distance;
-
- bool change = true;
- unsigned int steps = 0;
-
- while (change && steps < m_maxImproveSteps)
- {
- change = false;
- steps++;
-
- for (unsigned int i = 0 ; i < dim ; ++i)
- {
- bool better = true;
- bool increased = false;
- while (better)
- {
- better = false;
- double backup = state->values[i];
- state->values[i] += add;
- bool isS = goal_r->isSatisfied(state, distance);
- if ((wasSatisfied && !isS) || !si->satisfiesBounds(state))
- state->values[i] = backup;
- else
- {
- wasSatisfied = isS;
- if (*distance < bestDist)
- {
- better = true;
- change = true;
- increased = true;
- bestDist = *distance;
- }
- else
- state->values[i] = backup;
- }
- }
-
- if (!increased)
- {
- better = true;
- while (better)
- {
- better = false;
- double backup = state->values[i];
- state->values[i] -= add;
- bool isS = goal_r->isSatisfied(state, distance);
- if ((wasSatisfied && !isS) || !si->satisfiesBounds(state))
- state->values[i] = backup;
- else
- {
- wasSatisfied = isS;
- if (*distance < bestDist)
- {
- better = true;
- change = true;
- bestDist = *distance;
- }
- else
- state->values[i] = backup;
- }
- }
- }
- }
- }
-
- *distance = bestDist;
- return wasSatisfied && valid(state);
-}
Added: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp (rev 0)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/ik/src/HCIK.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* \author Ioan Sucan */
+
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
+#include <algorithm>
+
+bool ompl::HCIK::tryToImprove(SpaceInformationKinematic::StateKinematic_t state, double add, double *distance) const
+{
+ SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(m_si->getGoal());
+ unsigned int dim = m_si->getStateDimension();
+
+ if (!goal_r)
+ return false;
+
+ double tempDistance;
+ double initialDistance;
+ bool wasSatisfied = goal_r->isSatisfied(state, &initialDistance);
+ double bestDist = initialDistance;
+
+ bool change = true;
+ unsigned int steps = 0;
+
+ while (change && steps < m_maxImproveSteps)
+ {
+ change = false;
+ steps++;
+
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ bool better = true;
+ bool increased = false;
+ while (better)
+ {
+ better = false;
+ double backup = state->values[i];
+ state->values[i] += add;
+ bool isS = goal_r->isSatisfied(state, &tempDistance);
+ if ((wasSatisfied && !isS) || !m_si->satisfiesBounds(state))
+ state->values[i] = backup;
+ else
+ {
+ wasSatisfied = isS;
+ if (tempDistance < bestDist)
+ {
+ better = true;
+ change = true;
+ increased = true;
+ bestDist = tempDistance;
+ }
+ else
+ state->values[i] = backup;
+ }
+ }
+
+ if (!increased)
+ {
+ better = true;
+ while (better)
+ {
+ better = false;
+ double backup = state->values[i];
+ state->values[i] -= add;
+ bool isS = goal_r->isSatisfied(state, &tempDistance);
+ if ((wasSatisfied && !isS) || !m_si->satisfiesBounds(state))
+ state->values[i] = backup;
+ else
+ {
+ wasSatisfied = isS;
+ if (tempDistance < bestDist)
+ {
+ better = true;
+ change = true;
+ bestDist = tempDistance;
+ }
+ else
+ state->values[i] = backup;
+ }
+ }
+ }
+ }
+ }
+
+ if (distance)
+ *distance = bestDist;
+ return bestDist < initialDistance;
+}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/KPIECE1.h 2009-02-19 07:28:48 UTC (rev 11392)
@@ -41,6 +41,7 @@
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/datastructures/GridX.h"
#include "ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h"
+#include "ompl/extension/samplingbased/kinematic/extension/ik/HCIK.h"
#include <vector>
namespace ompl
@@ -73,7 +74,8 @@
{
public:
- KPIECE1(SpaceInformation_t si) : Planner(si)
+ KPIECE1(SpaceInformation_t si) : Planner(si),
+ m_hcik(dynamic_cast<SpaceInformationKinematic_t>(si))
{
m_type = PLAN_TO_GOAL_STATE | PLAN_TO_GOAL_REGION;
m_projectionEvaluator = NULL;
@@ -85,6 +87,7 @@
m_minValidPathPercentage = 0.2;
m_rho = 0.5;
m_tree.grid.onCellUpdate(computeImportance, NULL);
+ m_hcik.setMaxImproveSteps(50);
}
virtual ~KPIECE1(void)
@@ -264,6 +267,7 @@
bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
void computeCoordinates(const Motion* motion, Grid::Coord &coord);
+ HCIK m_hcik;
TreeData m_tree;
ProjectionEvaluator *m_projectionEvaluator;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/kpiece/src/KPIECE1.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -83,7 +83,8 @@
Motion *approxsol = NULL;
double approxdif = INFINITY;
SpaceInformationKinematic::StateKinematic_t xstate = new SpaceInformationKinematic::StateKinematic(dim);
-
+
+ double improveValue = 0.01;
unsigned int iteration = 1;
while (time_utils::Time::now() < endTime)
@@ -97,8 +98,25 @@
assert(existing);
/* sample random state (with goal biasing) */
- if (goal_s && m_rng.uniform(0.0, 1.0) < m_goalBias)
- si->copyState(xstate, goal_s->state);
+ if (m_rng.uniform(0.0, 1.0) < m_goalBias)
+ {
+ if (goal_s)
+ si->copyState(xstate, goal_s->state);
+ else
+ {
+ if (approxsol)
+ {
+ si->copyState(xstate, approxsol->state);
+ if (!m_hcik.tryToImprove(xstate, improveValue))
+ {
+ si->sampleNear(xstate, existing->state, range);
+ improveValue /= 2.0;
+ }
+ }
+ else
+ si->sampleNear(xstate, existing->state, range);
+ }
+ }
else
si->sampleNear(xstate, existing->state, range);
@@ -184,7 +202,7 @@
if (scell && !scell->data->motions.empty())
{
scell->data->selections++;
- smotion = scell->data->motions[m_rng.uniformInt(0, scell->data->motions.size() - 1)];
+ smotion = scell->data->motions[m_rng.halfNormalInt(0, scell->data->motions.size() - 1)];
return true;
}
else
@@ -221,7 +239,7 @@
cell->data->coverage = 1.0;
cell->data->iteration = iteration;
cell->data->selections = 1;
- cell->data->score = 1.0/ (1e-6 + dist);
+ cell->data->score = 1.0 / (1e-3 + dist);
m_tree.grid.add(cell);
created = 1;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 07:16:15 UTC (rev 11391)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-02-19 07:28:48 UTC (rev 11392)
@@ -90,7 +90,7 @@
req.params.model_id = GROUPNAME;
req.params.distance_metric = "L2Square";
- req.params.planner_id = "SBL";
+ req.params.planner_id = "KPIECE";
req.threshold = 0.1;
req.interpolate = 1;
req.times = 1;
@@ -140,14 +140,15 @@
req.goal_constraints[0].z = 0.829675;
req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].pitch = 0.0;
req.goal_constraints[0].yaw = 0.0;
- req.goal_constraints[0].position_distance = 0.001;
- req.goal_constraints[0].orientation_distance = 0.03;
- req.goal_constraints[0].orientation_importance = 0.01;
+ req.goal_constraints[0].position_distance = 0.0001;
+ req.goal_constraints[0].orientation_distance = 0.1;
+ req.goal_constraints[0].orientation_importance = 0.0001;
// allow 1 second computation time
- req.allowed_time = 1.0;
+ req.allowed_time = 0.5;
// define the service messages
robot_srvs::KinematicReplanLinkPosition::Request s_req;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups.xml
================================...
[truncated message content] |
Thanks for helping keep SourceForge clean.
X