|
From: <is...@us...> - 2008-09-02 22:55:08
|
Revision: 3879
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3879&view=rev
Author: isucan
Date: 2008-09-02 22:55:04 +0000 (Tue, 02 Sep 2008)
Log Message:
-----------
some documentation + changing message NamedKinematicPath to DisplayKinematicPath
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl/code/ompl/base/General.h
pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h
pkg/trunk/motion_planning/ompl/code/ompl/base/util/types.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/fake_localization/manifest.xml
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg
Removed Paths:
-------------
pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/General.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/General.h 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/General.h 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
/**
@@ -66,25 +66,25 @@
<hr>
- @section Sampling-based motion planners
+ @section sampling_planners Sampling-based motion planners
- This class of motion planner typically needs the ability to sample
+ This class of motion planners typically needs the ability to sample
the state (configuration) space of the robot(s) planning is done
for. To allow this, an implementation of StateValidityChecker must
be provided. This implementation will most likely depend on a
collision detector.
- @subsection Kinematic motion planners
+ @subsection kinematic_planners Kinematic motion planners
- @ref RRT
- @ref LazyRRT
- @ref SBL
- @subsection Kinodynamic motion planners
+ @subsection kinodynamic_planners Kinodynamic motion planners
- None implemented yet
- @section Grid-based motion planners
+ @section grid_planners Grid-based motion planners
- Not included yet
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/src/time.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** This file is taken from ROS, adaptations by Ioan Sucan */
+/* This file is taken from ROS, adaptations by Ioan Sucan */
#include "ompl/base/util/time.h"
#include <cmath>
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/time.h 2008-09-02 22:55:04 UTC (rev 3879)
@@ -36,7 +36,7 @@
#ifndef OMPL_TIME_
#define OMPL_TIME_
-/** This file is taken from ROS, adaptations by Ioan Sucan */
+/* This file is taken from ROS, adaptations by Ioan Sucan */
#include <iostream>
#include <cmath>
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/util/types.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/util/types.h 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/util/types.h 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** This file is taken from ROS */
+/* This file is taken from ROS */
#ifndef OMPL_TYPES_
#define OMPL_TYPES_
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
#ifndef OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_LAZY_RRT_
#define OMPL_EXTENSION_SAMPLINGBASED_KINEMATIC_EXTENSION_LAZY_RRT_
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,14 +32,19 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
/**
- @subsubsection RRT (Rapidly-exploring Random Trees)
+ @subsubsection RRT Rapidly-exploring Random Trees (RRT)
@par Short description
-
+ The basic idea of RRT is that it samples a random state qr in the
+ state space, then finds the state qc among the previously seen
+ states that is closest to qr and expands from qc towards qr, until
+ a state qm is reached and qm is the new state to be visited.
+
+
@par External documentation
@link http://en.wikipedia.org/wiki/Rapidly-exploring_random_tree
@link http://msl.cs.uiuc.edu/~lavalle/rrtpubs.html
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
#include "ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h"
#include <cassert>
Modified: 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/rrt/src/RRT.cpp 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
#include "ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h"
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 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -33,7 +33,7 @@
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
/** This is a simple program for requesting a motion plan */
@@ -41,7 +41,7 @@
#include <ros/time.h>
#include <robot_srvs/KinematicPlanState.h>
#include <robot_srvs/KinematicPlanLinkPosition.h>
-#include <robot_msgs/NamedKinematicPath.h>
+#include <robot_msgs/DisplayKinematicPath.h>
#include <std_msgs/RobotBase2DOdom.h>
class PlanKinematicPath : public ros::node
@@ -50,7 +50,7 @@
PlanKinematicPath(void) : ros::node("plan_kinematic_path")
{
- advertise<robot_msgs::NamedKinematicPath>("display_kinematic_path", 1);
+ advertise<robot_msgs::DisplayKinematicPath>("display_kinematic_path", 1);
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_haveBasePos = false;
@@ -78,7 +78,7 @@
req.params.model_id = "pr2::base";
req.params.distance_metric = "L2Square";
- req.params.planner_id = "SBL";
+ req.params.planner_id = "LazyRRT";
req.threshold = 0.01;
req.interpolate = 1;
req.times = 1;
@@ -159,8 +159,9 @@
void performCall(robot_srvs::KinematicPlanState::request &req)
{
robot_srvs::KinematicPlanState::response res;
- robot_msgs::NamedKinematicPath dpath;
-
+ robot_msgs::DisplayKinematicPath dpath;
+ dpath.frame_id = "FRAMEID_MAP";
+
if (ros::service::call("plan_kinematic_path_state", req, res))
{
unsigned int nstates = res.path.get_states_size();
@@ -227,8 +228,9 @@
void performCall(robot_srvs::KinematicPlanLinkPosition::request &req)
{
robot_srvs::KinematicPlanLinkPosition::response res;
- robot_msgs::NamedKinematicPath dpath;
-
+ robot_msgs::DisplayKinematicPath dpath;
+ dpath.frame_id = "FRAMEID_MAP";
+
if (ros::service::call("plan_kinematic_path_position", req, res))
{
unsigned int nstates = res.path.get_states_size();
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-/** \Author Ioan Sucan */
+/** \author Ioan Sucan */
/**
@@ -89,7 +89,7 @@
#include <planning_node_util/cnode.h>
#include <rosthread/mutex.h>
-#include <robot_msgs/NamedKinematicPath.h>
+#include <robot_msgs/DisplayKinematicPath.h>
#include <display_ode/displayODE.h>
#include <vector>
@@ -273,7 +273,7 @@
display_ode::DisplayODESpaces m_spaces;
ros::thread::mutex m_displayLock;
- robot_msgs::NamedKinematicPath m_displayPath;
+ robot_msgs::DisplayKinematicPath m_displayPath;
bool m_follow;
bool m_displayRobot;
bool m_displayObstacles;
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-02 22:55:04 UTC (rev 3879)
@@ -54,7 +54,8 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "odom"/RobotBase2DOdom : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pos"/Pose3DEulerFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "initialpose"/Pose2DFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
- @b "localizedpose"/RobotBase2DOdom : robot's localized map pose. Only the position information is set (no velocity).
@@ -74,6 +75,7 @@
#include <std_msgs/RobotBase2DOdom.h>
#include <std_msgs/ParticleCloud2D.h>
#include <std_msgs/Pose2DFloat32.h>
+#include <robot_msgs/Pose3DEulerFloat32.h>
#include <math_utils/angles.h>
#include <rosTF/rosTF.h>
@@ -95,7 +97,8 @@
m_particleCloud.set_particles_size(1);
param("max_publish_frequency", m_maxPublishFrequency, 0.5);
- subscribe("odom", m_odomMsg, &FakeOdomNode::odomReceived);
+
+ subscribe("base_pos", m_basePosMsg, &FakeOdomNode::basePosReceived);
subscribe("initialpose", m_iniPos, &FakeOdomNode::initialPoseReceived);
}
@@ -108,14 +111,14 @@
private:
- rosTFServer *m_tfServer;
- ros::Time m_lastUpdate;
- double m_maxPublishFrequency;
+ rosTFServer *m_tfServer;
+ ros::Time m_lastUpdate;
+ double m_maxPublishFrequency;
- std_msgs::RobotBase2DOdom m_odomMsg;
- std_msgs::ParticleCloud2D m_particleCloud;
- std_msgs::RobotBase2DOdom m_currentOdom;
- std_msgs::Pose2DFloat32 m_iniPos;
+ robot_msgs::Pose3DEulerFloat32 m_basePosMsg;
+ std_msgs::ParticleCloud2D m_particleCloud;
+ std_msgs::RobotBase2DOdom m_currentPos;
+ std_msgs::Pose2DFloat32 m_iniPos;
void initialPoseReceived(void)
@@ -123,7 +126,7 @@
update();
}
- void odomReceived(void)
+ void basePosReceived(void)
{
update();
}
@@ -135,26 +138,29 @@
m_lastUpdate = ros::Time::now();
- m_currentOdom = m_odomMsg;
+ m_currentPos.header = m_basePosMsg.header;
+ m_currentPos.pos.x = m_basePosMsg.position.x;
+ m_currentPos.pos.y = m_basePosMsg.position.y;
+ m_currentPos.pos.th = m_basePosMsg.orientation.yaw;
- m_currentOdom.pos.x += m_iniPos.x;
- m_currentOdom.pos.y += m_iniPos.y;
- m_currentOdom.pos.th = math_utils::normalize_angle(m_currentOdom.pos.th + m_iniPos.th);
- m_currentOdom.header.frame_id = "FRAMEID_MAP";
+ m_currentPos.pos.x += m_iniPos.x;
+ m_currentPos.pos.y += m_iniPos.y;
+ m_currentPos.pos.th = math_utils::normalize_angle(m_currentPos.pos.th + m_iniPos.th);
+ m_currentPos.header.frame_id = "FRAMEID_MAP";
m_tfServer->sendEuler("FRAMEID_ROBOT",
"FRAMEID_MAP",
- m_currentOdom.pos.x,
- m_currentOdom.pos.y,
+ m_currentPos.pos.x,
+ m_currentPos.pos.y,
0.0,
- m_currentOdom.pos.th,
+ m_currentPos.pos.th,
0.0,
0.0,
- m_currentOdom.header.stamp);
+ m_currentPos.header.stamp);
- publish("localizedpose", m_currentOdom);
+ publish("localizedpose", m_currentPos);
- m_particleCloud.particles[0] = m_currentOdom.pos;
+ m_particleCloud.particles[0] = m_currentPos.pos;
publish("particlecloud", m_particleCloud);
}
Modified: pkg/trunk/nav/fake_localization/manifest.xml
===================================================================
--- pkg/trunk/nav/fake_localization/manifest.xml 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/nav/fake_localization/manifest.xml 2008-09-02 22:55:04 UTC (rev 3879)
@@ -3,7 +3,7 @@
<author>Ioan A. Sucan</author>
<license>BSD</license>
<depend package="roscpp" />
- <depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="rosTF" />
<depend package="math_utils" />
</package>
Added: pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/DisplayKinematicPath.msg 2008-09-02 22:55:04 UTC (rev 3879)
@@ -0,0 +1,5 @@
+# The definition of a kinematic path that has a name
+# The name identifies the part of the robot the path is for
+string frame_id
+string name
+KinematicPath path
Deleted: pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg 2008-09-02 22:20:42 UTC (rev 3878)
+++ pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg 2008-09-02 22:55:04 UTC (rev 3879)
@@ -1,5 +0,0 @@
-# The definition of a kinematic path that has a name
-# The name identifies the part of the robot the path is for
-# This is useful in displaying paths
-string name
-KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|