Menu
▾
▴
personalrobots-commit
|
From: <hsu...@us...> - 2009-04-16 22:32:20
|
Revision: 13982
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13982&view=rev
Author: hsujohnhsu
Date: 2009-04-16 22:32:09 +0000 (Thu, 16 Apr 2009)
Log Message:
-----------
- bug fix: update gazebo inertia matrix setup, dMassTranslate not needed.
- update dual link example.
- update multi link example.
- update gripper, added fake inertia scale so the large k_velocity/k_position will stablize in sim.
- update urdf2gazebo, joint limit enforced from [-PI,PI]. previously [-.9*PI,.9*PI].
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2009-04-16 22:32:09 UTC (rev 13982)
@@ -262,7 +262,40 @@
===================================================================
--- server/physics/Body.cc (revision 7599)
+++ server/physics/Body.cc (working copy)
-@@ -812,7 +812,6 @@
+@@ -809,15 +809,42 @@
+ this->comPose.pos.y = this->cy;
+ this->comPose.pos.z = this->cz;
+
++#if 0
++ // DEBUG: TESTING DYNAMICS, THIS SECTION IS NOT NEEDED
++ // apply parallel axis theorem, Huygens-Steiner Theorem, move CG to body ref frame
++ double ixx1,iyy1,izz1,ixy1,ixz1,iyz1;
++ double cx2 = pow(cx,2);
++ double cy2 = pow(cy,2);
++ double cz2 = pow(cz,2);
++ ixx1 = ixx + this->bodyMass*(cy2+cz2);
++ iyy1 = iyy + this->bodyMass*(cx2+cz2);
++ izz1 = izz + this->bodyMass*(cx2+cy2);
++ ixy1 = ixy - this->bodyMass*(cx*cy);
++ ixz1 = ixz - this->bodyMass*(cx*cz);
++ iyz1 = iyz - this->bodyMass*(cy*cz);
++
++ this->ixx = ixx1;
++ this->iyy = iyy1;
++ this->izz = izz1;
++ this->ixy = ixy1;
++ this->ixz = ixz1;
++ this->iyz = iyz1;
++ this->cx = this->cx - this->cx;
++ this->cy = this->cy - this->cy;
++ this->cz = this->cz - this->cz;
++#endif
++
++ // apply same hack as before, move cg to 0,0,0, taken care of by comPose.
++ this->cx = 0.0;
++ this->cy = 0.0;
++ this->cz = 0.0;
++
// setup this->mass as well
dMassSetParameters(&this->mass, this->bodyMass,
this->cx, this->cy, this->cz,
@@ -270,7 +303,12 @@
this->ixx,this->iyy,this->izz,
this->ixy,this->ixz,this->iyz);
-@@ -825,19 +824,27 @@
+- dMassTranslate( &this->mass, -this->cx, -this->cy, -this->cz);
+-
+ // dMatrix3 rot;
+ // dMassRotate(&this->mass, rot);
+
+@@ -825,19 +852,32 @@
if (this->mass.mass > 0)
dBodySetMass( this->bodyId, &this->mass );
@@ -286,6 +324,7 @@
- // std::cout << " I[6] " << this->mass.I[6] << std::endl;
- // std::cout << " I[7] " << this->mass.I[7] << std::endl;
- // std::cout << " I[8] " << this->mass.I[8] << std::endl;
++#define DEBUG
+#ifdef DEBUG
+#define _I(i,j) I[(i)*4+(j)]
+ std::cout << " name: " << this->GetName() << std::endl;
@@ -304,7 +343,11 @@
+ std::cout << " I(2,2) " << this->mass._I(2,2) << std::endl;
+ // check and make sure no inertias less than zero. if less than zero, the inertia matrix + mass + cg combo is a bad one.
-+ assert(this->mass._I(0,0)>0.0 && this->mass._I(1,1)>0.0 && this->mass._I(2,2)>0.0);
++ if (this->mass._I(0,0) < 0.0 || this->mass._I(1,1) < 0.0 || this->mass._I(2,2) < 0.0)
++ {
++ //assert(0);
++ std::cout << "ERROR: ixx,iyy or izz < 0" << std::endl;
++ }
+#endif
+
}
Modified: pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/demos/examples_gazebo/dual_link_defs/dual_link.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -21,21 +21,19 @@
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<limit min="0.0" max="0.0" effort="100" velocity="5"
- k_position="1.0" k_velocity="1.0"
+ k_position="0.0" k_velocity="0.0"
safety_length_min="0.0" safety_length_max="0.0" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="0.0" />
</joint>
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="0.0 0 0" />
- <limit min="-3.0" max="3.0" effort="100" velocity="5"
- k_position="1.0" k_velocity="1.0"
- safety_length_min="0.0" safety_length_max="0.0" />
+ <anchor xyz="0 0 0" />
+ <limit effort="100" velocity="5" k_velocity="1000" />
<calibration values="1.5 -1 " />
- <joint_properties damping="0.0" />
+ <joint_properties damping="0.0" friction="0.0" />
</joint>
@@ -102,9 +100,9 @@
<origin xyz="0.0 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
- <mass value="1.0" />
- <com xyz="0.1 0 0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+ <mass value="10.0" />
+ <com xyz="0.5 0 0" />
+ <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="10.0" iyz="0.0" izz="0.01" />
</inertial>
<visual >
<origin xyz="0.5 0 0" rpy="0 0 0" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/demos/examples_gazebo/multi_link_defs/multi_link.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -21,77 +21,63 @@
<joint name="link1_joint" type="revolute" >
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.1" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.1" damping_constant="0.0" />
- <limit k_position="1.0" k_velocity="1.0" min="-.5" max="0.5" effort="10" velocity="50" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-.5" max="0.5" effort="10" velocity="50" />
<calibration values="1.5 -1 " />
- <joint_properties damping="200.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="5000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="5000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="100.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link3_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="5000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="5000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="100.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link4_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="2000" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="2000" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link5_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="500" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="500" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link6_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="100" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- joint blocks -->
<joint name="link7_joint" type="revolute" >
<axis xyz="0 1 0" />
<anchor xyz="0.0 0 0" />
- <safety_limit_min spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <safety_limit_max spring_constant="0.0" safety_length="0.01" damping_constant="0.0" />
- <limit k_position="100.0" k_velocity="100.0" min="-1" max="2" effort="100" velocity="5" />
+ <limit safety_length_min="0" safety_length_max="0" k_position="1.0" k_velocity="1.0" min="-1" max="2" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
- <joint_properties damping="10.0" />
+ <joint_properties damping="1.0" />
</joint>
<!-- link blocks -->
@@ -126,9 +112,9 @@
<origin xyz="0 0 10" rpy="0 0 0" />
<joint name="link1_joint" />
<inertial >
- <mass value="1" />
+ <mass value="100.0" />
<com xyz="0 0 -0.5" />
- <inertia ixx="2.00" ixy="0.0" ixz="0.0" iyy="2.0" iyz="0.0" izz="2.0" />
+ <inertia ixx="200.00" ixy="0.0" ixz="0.0" iyy="200.0" iyz="0.0" izz="100.0" />
</inertial>
<visual >
<origin xyz="0 0 -0.5" rpy="0 0 0" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2gazebo.cpp 2009-04-16 22:32:09 UTC (rev 13982)
@@ -369,20 +369,20 @@
ROS_WARN("urdf2gazebo: limiting lowStop to <= 0 degrees");
*lowstop = 0.0;
}
- if (*lowstop < -(M_PI*0.9))
+ if (*lowstop < -(M_PI))
{
- ROS_WARN("urdf2gazebo: limiting lowStop to >= -(180*0.9) degrees");
- *lowstop = -(M_PI*0.9);
+ ROS_WARN("urdf2gazebo: limiting lowStop to >= -(180) degrees");
+ *lowstop = -(M_PI);
}
if (*highstop < 0)
{
ROS_WARN("urdf2gazebo: limiting highStop to >= 0 degrees");
*highstop = 0.0;
}
- if (*highstop > (M_PI*0.9))
+ if (*highstop > (M_PI))
{
- ROS_WARN("urdf2gazebo: limiting highStop to <= (180*0.9) degrees");
- *highstop = (M_PI*0.9);
+ ROS_WARN("urdf2gazebo: limiting highStop to <= (180) degrees");
+ *highstop = (M_PI);
}
addKeyValue(joint, "lowStop", values2str(1, link->joint->limit , rad2deg));
addKeyValue(joint, "highStop", values2str(1, link->joint->limit + 1, rad2deg));
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-04-16 22:27:12 UTC (rev 13981)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/gripper_defs.xml 2009-04-16 22:32:09 UTC (rev 13982)
@@ -4,6 +4,7 @@
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
<property name="M_PI" value="3.1415926535897931" />
+ <property name="scale" value="10.0" />
<property name="gripper_max_angle" value="0.548" /> <!-- max gap size /2 -->
<property name="gripper_min_angle" value="0.00" />
@@ -12,7 +13,7 @@
<property name="finger_stop_kp" value="100000.0" />
<property name="finger_fudge_factor" value="1.0" />
- <property name="finger_damping" value="3.0" />
+ <property name="finger_damping" value="0.001" />
<property name="finger_tip_damping" value="0.001" />
<property name="finger_kp" value="0.0" />
@@ -32,7 +33,7 @@
<!-- IMPORTANT: k_velocity and k_position of 1 will induce instability on the acutal joint -->
<limit effort="${finger_joint_effort_limit}" velocity="0.5"
min="${gripper_min_angle}" max="${gripper_max_angle}"
- k_velocity="0.5" k_position="0.5"
+ k_velocity="1.0" k_position="1.0"
safety_length_min="0.0" safety_length_max="0.01" />
</macro>
@@ -65,12 +66,12 @@
<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" />
+ <inertia ixx= "{scale*0.00007756198}"
+ ixy= "{scale*0.00000149095}"
+ ixz="{-scale*0.00000983385}"
+ iyy= "{scale*0.00019708305}"
+ iyz="{-scale*0.00000306125}"
+ izz= "{scale*0.00018105446}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -147,12 +148,12 @@
<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" />
+ <inertia ixx= "{scale*0.00007738410}"
+ ixy="{-scale*0.00000209309}"
+ ixz="{-scale*0.00000836228}"
+ iyy= "{scale*0.00019847383}"
+ iyz= "{scale*0.00000246110}"
+ izz= "{scale*0.00018106988}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -229,9 +230,9 @@
<inertial>
<mass value="0.04419" />
<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" />
+ <inertia ixx="{scale*0.00000837047}" ixy="{scale*0.00000583632}" ixz="{scale*0.0}"
+ iyy="{scale*0.00000987067}" iyz="{scale*0.0}"
+ izz="{scale*0.00001541768}" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -309,9 +310,9 @@
<inertial>
<mass value="0.04419" />
<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" />
+ <inertia ixx="{scale*0.00000837047)" ixy="{-scale*0.00000583632)" ixz="{scale*0.0)"
+ iyy="{scale*0.00000987067)" iyz="{scale*0.0)"
+ izz="{scale*0.00001541768)" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
@@ -580,10 +581,10 @@
<actuator name="${side}_gripper_motor" />
<gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
<passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.001" i="0.0" d="0.0" iClamp="0.0" />
+ <gazebo_mimic_pid p="0.0001" i="0.0" d="0.0" iClamp="0.0" />
</passive_joint>
<passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.001" i="0.0" d="0.0" iClamp="0.0" />
+ <gazebo_mimic_pid p="0.0001" i="0.0" d="0.0" iClamp="0.0" />
</passive_joint>
<passive_joint name="${side}_gripper_r_finger_tip_joint" >
<gazebo_mimic_pid p="0.00001" i="0.0" d="0.0" iClamp="0.0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-04-17 00:15:40
|
Revision: 13985
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13985&view=rev
Author: stuglaser
Date: 2009-04-17 00:15:33 +0000 (Fri, 17 Apr 2009)
Log Message:
-----------
Updated to use the conversion methods in the tf_conversions package.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/chain.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2009-04-17 00:15:33 UTC (rev 13985)
@@ -16,6 +16,7 @@
<depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="tf" />
+ <depend package="tf_conversions" />
<depend package="kdl" />
<depend package="angles" />
<depend package="joy" />
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-04-17 00:15:33 UTC (rev 13985)
@@ -37,6 +37,7 @@
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/jacobian.hpp>
+#include "tf_conversions/tf_kdl.h"
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
#include "robot_msgs/VisualizationMarker.h"
@@ -49,7 +50,7 @@
void TransformKDLToMsg(const KDL::Frame &k, robot_msgs::Pose &m)
{
tf::Transform tf;
- mechanism::TransformKDLToTF(k, tf);
+ tf::TransformKDLToTF(k, tf);
tf::PoseTFToMsg(tf, m);
}
@@ -443,7 +444,7 @@
pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
tf::Transform t;
- mechanism::TransformKDLToTF(c_.tool_frame_offset_, t);
+ tf::TransformKDLToTF(c_.tool_frame_offset_, t);
tf::TransformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
pub_tf_->unlockAndPublish();
}
@@ -470,7 +471,7 @@
ROS_WARN("Transform Exception %s", ex.what());
return;
}
- mechanism::TransformTFToKDL(task_frame, c_.task_frame_offset_);
+ tf::TransformTFToKDL(task_frame, c_.task_frame_offset_);
c_.mode_[0] = (int)command_msg_.mode.vel.x;
c_.mode_[1] = (int)command_msg_.mode.vel.y;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-04-17 00:15:33 UTC (rev 13985)
@@ -34,6 +34,7 @@
#include "urdf/parser.h"
#include <algorithm>
#include "robot_mechanism_controllers/plug_controller.h"
+#include "tf_conversions/tf_kdl.h"
#define DEBUG 0 // easy debugging
@@ -72,9 +73,9 @@
{
assert(robot);
robot_ = robot;
-
+
controller_name_ = config->Attribute("name");
-
+
TiXmlElement *chain = config->FirstChildElement("chain");
if (!chain) {
ROS_ERROR("PlugController was not given a chain");
@@ -108,13 +109,13 @@
node->param(controller_name_+"/f_limit_max" , f_limit_max , 100.0) ; /// max upper arm limit force
node->param(controller_name_+"/upper_arm_dead_zone", upper_arm_dead_zone, 0.05);
- roll_pid_.initParam(controller_name_+"/pose_pid");
- pitch_pid_.initParam(controller_name_+"/pose_pid");
- yaw_pid_.initParam(controller_name_+"/pose_pid");
- line_pid_.initParam(controller_name_+"/line_pid");
+ roll_pid_.initParam(controller_name_+"/pose_pid");
+ pitch_pid_.initParam(controller_name_+"/pose_pid");
+ yaw_pid_.initParam(controller_name_+"/pose_pid");
+ line_pid_.initParam(controller_name_+"/line_pid");
last_time_ = robot->model_->hw_->current_time_;
-
-
+
+
// Constructs solvers and allocates matrices.
unsigned int num_joints = kdl_chain_.getNrOfJoints();
jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
@@ -335,7 +336,7 @@
chain_.toKDL(new_kdl_chain);
KDL::Frame tool_frame;
- mechanism::TransformTFToKDL(tool_offset, tool_frame);
+ tf::TransformTFToKDL(tool_offset, tool_frame);
new_kdl_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), tool_frame));
kdl_chain_ = new_kdl_chain;
@@ -391,7 +392,7 @@
if (current_frame_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete current_frame_publisher_ ;
current_frame_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::Transform> (topic_ + "/transform", 1) ;
-
+
if (internal_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete internal_state_publisher_ ;
internal_state_publisher_ = new realtime_tools::RealtimePublisher <robot_mechanism_controllers::PlugInternalState> (topic_ + "/internal_state", 1) ;
@@ -413,7 +414,7 @@
if (current_frame_publisher_->trylock())
{
tf::Transform transform;
- mechanism::TransformKDLToTF(controller_.endeffector_frame_, transform);
+ tf::TransformKDLToTF(controller_.endeffector_frame_, transform);
tf::TransformTFToMsg(transform, current_frame_publisher_->msg_);
current_frame_publisher_->unlockAndPublish() ;
}
@@ -457,7 +458,7 @@
controller_.outlet_norm_(1) = norm.y();
controller_.outlet_norm_(2) = norm.z();
- mechanism::TransformTFToKDL(outlet, controller_.desired_frame_);
+ tf::TransformTFToKDL(outlet, controller_.desired_frame_);
}
void PlugControllerNode::command()
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-04-17 00:15:33 UTC (rev 13985)
@@ -40,11 +40,6 @@
namespace mechanism {
-void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k);
-void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t);
-void RotationTFToKDL(const tf::Quaternion& t, KDL::Rotation& k);
-void VectorTFToKDL(const tf::Vector3& t, KDL::Vector& k);
-
class Chain
{
public:
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2009-04-17 00:15:33 UTC (rev 13985)
@@ -9,6 +9,7 @@
<depend package="tinyxml" />
<depend package="wg_robot_description_parser" />
<depend package="tf" />
+<depend package="tf_conversions" />
<depend package="std_srvs" />
<depend package="control_toolbox" />
<depend package="bullet" />
Modified: pkg/trunk/mechanism/mechanism_model/src/chain.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/chain.cpp 2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/src/chain.cpp 2009-04-17 00:15:33 UTC (rev 13985)
@@ -30,37 +30,10 @@
// Author: Stuart Glaser
#include "mechanism_model/chain.h"
+#include "tf_conversions/tf_kdl.h"
-
namespace mechanism {
-void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k)
-{
- for (unsigned int i = 0; i < 3; ++i)
- k.p.data[i] = t.getOrigin()[i];
- for (unsigned int i = 0; i < 9; ++i)
- k.M.data[i] = t.getBasis()[i/3][i%3];
-}
-
-void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t)
-{
- t.setOrigin(tf::Vector3(k.p.data[0], k.p.data[1], k.p.data[2]));
- t.setBasis(btMatrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
- k.M.data[3], k.M.data[4], k.M.data[5],
- k.M.data[6], k.M.data[7], k.M.data[8]));
-}
-
-void RotationTFToKDL(const tf::Quaternion& t, KDL::Rotation& k)
-{
- k.Quaternion(t[0], t[1], t[2], t[3]);
-}
-
-void VectorTFToKDL(const tf::Vector3& t, KDL::Vector& k)
-{
- k = KDL::Vector(t[0], t[1], t[2]);
-}
-
-
bool Chain::init(Robot *robot, const std::string &root, const std::string &tip)
{
robot_ = robot;
@@ -163,32 +136,32 @@
for (i = 0; i < link_indices_.size(); ++i)
{
// Here we create a KDL Segment for each part of the chain, between root and tip.
- // A KDL Segment consists of a link and a joint. The joint is positioned at the
+ // A KDL Segment consists of a link and a joint. The joint is positioned at the
// beginning of the link, and connects this link to the previous link.
KDL::Frame kdl_link;
KDL::Vector pos;
KDL::Rotation rot;
KDL::Vector axis;
- // Creates the link: a link is defined by a position and rotation,
+ // Creates the link: a link is defined by a position and rotation,
// relative to the reference frame of the previous link.
if (i == 0){
kdl_link = KDL::Frame::Identity();
}
// moving from root to ancestor
else if (i<= reversed_index_){
- VectorTFToKDL(robot_->links_[link_indices_[i-1]]->getOffset().getOrigin(), pos);
- RotationTFToKDL(robot_->links_[link_indices_[i-1]]->getRotation().getRotation(), rot);
+ tf::VectorTFToKDL(robot_->links_[link_indices_[i-1]]->getOffset().getOrigin(), pos);
+ tf::RotationTFToKDL(robot_->links_[link_indices_[i-1]]->getRotation().getRotation(), rot);
kdl_link = KDL::Frame(rot, pos).Inverse();
}
// moving from ancestor to tip
else{
- VectorTFToKDL(robot_->links_[link_indices_[i]]->getOffset().getOrigin(), pos);
- RotationTFToKDL(robot_->links_[link_indices_[i]]->getRotation().getRotation(), rot);
+ tf::VectorTFToKDL(robot_->links_[link_indices_[i]]->getOffset().getOrigin(), pos);
+ tf::RotationTFToKDL(robot_->links_[link_indices_[i]]->getRotation().getRotation(), rot);
kdl_link = KDL::Frame(rot, pos);
}
- // Creates the joint: a joint is defined by a position and an axis,
+ // Creates the joint: a joint is defined by a position and an axis,
// relative to the reference frame of the previous link.
KDL::Joint kdl_joint;
if (i == 0 || robot_->joints_[all_joint_indices_[i-1]]->type_ == JOINT_FIXED){
@@ -196,12 +169,12 @@
}
// moving from ancestor to tip
else if (i<= reversed_index_){
- VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
+ tf::VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
kdl_joint = KDL::Joint(KDL::Vector::Zero(), kdl_link.M * axis * -1, KDL::Joint::RotAxis);
}
// moving from ancestor to tip
else{
- VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
+ tf::VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
kdl_joint = KDL::Joint(kdl_link.p, axis, KDL::Joint::RotAxis);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-04-17 14:47:02
|
Revision: 14002
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14002&view=rev
Author: gerkey
Date: 2009-04-17 14:46:53 +0000 (Fri, 17 Apr 2009)
Log Message:
-----------
Blacklisted vo and friends
Added Paths:
-----------
pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST
pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST
pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST
Added: pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST 2009-04-17 14:46:53 UTC (rev 14002)
@@ -0,0 +1 @@
+depends on visual_odometry
Added: pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST 2009-04-17 14:46:53 UTC (rev 14002)
@@ -0,0 +1 @@
+depends on visual_odometry
Added: pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST 2009-04-17 14:46:53 UTC (rev 14002)
@@ -0,0 +1,12 @@
+Building CXX object CMakeFiles/visual_odometry.dir/src/CvMatUtils.o
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/vision/visual_odometry/src/CvMatUtils.cpp:
+In static member function 'static bool
+cv::willow::CvMatUtils::getVisualizableDisparityMap(cv::WImage1_16s&,
+cv::WImage3_b&, double)':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/vision/visual_odometry/src/CvMatUtils.cpp:98:
+error: invalid initialization of reference of type 'const cv::Mat&' from
+expression of type 'double'
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/3rdparty/opencv_latest/opencv/include/opencv/cxcore.hpp:889:
+error: in passing argument 1 of 'void cv::pow(const cv::Mat&, double,
+cv::Mat&)'
+make[3]: *** [CMakeFiles/visual_odometry.dir/src/CvMatUtils.o] Error 1
Added: pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST 2009-04-17 14:46:53 UTC (rev 14002)
@@ -0,0 +1 @@
+depends on visual_odometry
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-04-17 19:14:54
|
Revision: 14023
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14023&view=rev
Author: gerkey
Date: 2009-04-17 19:14:38 +0000 (Fri, 17 Apr 2009)
Log Message:
-----------
Unblacklisting packages that depend on visual_odometry
Removed Paths:
-------------
pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST
pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST
pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST
Deleted: pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST 2009-04-17 19:10:27 UTC (rev 14022)
+++ pkg/trunk/nav/visual_nav/ROS_BUILD_BLACKLIST 2009-04-17 19:14:38 UTC (rev 14023)
@@ -1 +0,0 @@
-depends on visual_odometry
Deleted: pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST 2009-04-17 19:10:27 UTC (rev 14022)
+++ pkg/trunk/vision/place_recognition/ROS_BUILD_BLACKLIST 2009-04-17 19:14:38 UTC (rev 14023)
@@ -1 +0,0 @@
-depends on visual_odometry
Deleted: pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST 2009-04-17 19:10:27 UTC (rev 14022)
+++ pkg/trunk/vision/vslam/ROS_BUILD_BLACKLIST 2009-04-17 19:14:38 UTC (rev 14023)
@@ -1 +0,0 @@
-depends on visual_odometry
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-17 21:40:00
|
Revision: 14032
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14032&view=rev
Author: eitanme
Date: 2009-04-17 21:39:55 +0000 (Fri, 17 Apr 2009)
Log Message:
-----------
Provide feedback in move base action, rotate to goal in controller and not move base action
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-17 21:39:55 UTC (rev 14032)
@@ -45,7 +45,6 @@
#include <costmap_2d/costmap_2d.h>
#include <navfn/navfn_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>
-#include <angles/angles.h>
#include <vector>
#include <string>
@@ -108,34 +107,6 @@
void prunePlan();
/**
- * @brief Once a goal is reached... rotate to the goal orientation
- * @param cmd_vel The velocity commands to be filled
- */
- void rotateToGoal(robot_msgs::PoseDot& cmd_vel);
-
- /**
- * @brief Check if the goal orientation has been achieved
- * @return True if achieved, false otherwise
- */
- bool goalOrientationReached();
-
- /**
- * @brief Check if the goal position has been achieved
- * @return True if achieved, false otherwise
- */
- bool goalPositionReached();
-
- /**
- * @brief Compute the distance between two points
- * @param x1 The first x point
- * @param y1 The first y point
- * @param x2 The second x point
- * @param y2 The second y point
- * @return
- */
- double distance(double x1, double y1, double x2, double y2);
-
- /**
* @brief Get the current pose of the robot in the global frame and set the global_pose_ variable
*/
void updateGlobalPose();
Modified: pkg/trunk/highlevel/nav/manifest.xml
===================================================================
--- pkg/trunk/highlevel/nav/manifest.xml 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/manifest.xml 2009-04-17 21:39:55 UTC (rev 14032)
@@ -17,7 +17,6 @@
<depend package="tf"/>
<depend package="roslib"/>
<depend package="navfn"/>
- <depend package="angles"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnav"/>
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-17 21:39:55 UTC (rev 14032)
@@ -169,6 +169,9 @@
std::vector<robot_actions::Pose2D> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
+ //we'll also push the goal point onto the end of the plan to make sure orientation is taken into account
+ global_plan.push_back(goal);
+
lock_.lock();
//copy over the new global plan
valid_plan_ = valid_plan;
@@ -198,31 +201,6 @@
}
}
- double MoveBaseAction::distance(double x1, double y1, double x2, double y2){
- return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
- }
-
- bool MoveBaseAction::goalPositionReached(){
- double dist = distance(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), goal_.x, goal_.y);
- return fabs(dist) <= xy_goal_tolerance_;
- }
-
- bool MoveBaseAction::goalOrientationReached(){
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- return fabs(angles::shortest_angular_distance(yaw, goal_.th)) <= yaw_goal_tolerance_;
- }
-
- void MoveBaseAction::rotateToGoal(robot_msgs::PoseDot& cmd_vel){
- double uselessPitch, uselessRoll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
- ROS_DEBUG("Moving to desired goal orientation\n");
- cmd_vel.vel.vx = 0;
- cmd_vel.vel.vy = 0;
- double ang_diff = angles::shortest_angular_distance(yaw, goal_.th);
- cmd_vel.ang_vel.vz = ang_diff > 0.0 ? std::max(min_abs_theta_vel_, ang_diff) : std::min(-1.0 * min_abs_theta_vel_, ang_diff);
- }
-
void MoveBaseAction::prunePlan(){
lock_.lock();
std::vector<robot_actions::Pose2D>::iterator it = global_plan_.begin();
@@ -259,6 +237,20 @@
//update the global pose
updateGlobalPose();
+ //update feedback to correspond to our current position
+ double useless_pitch, useless_roll, yaw;
+ global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
+ feedback.header.frame_id = global_frame_;
+ feedback.header.stamp = ros::Time::now();
+ feedback.x = global_pose_.getOrigin().x();
+ feedback.y = global_pose_.getOrigin().y();
+ feedback.z = 0.0;
+ feedback.th = yaw;
+
+ //push the feedback out
+ update(feedback);
+
+
//make sure to update the cost_map we'll use for this cycle
controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
@@ -268,61 +260,51 @@
//prune the plan before we pass it to the controller
prunePlan();
- //check for success
- if(goalPositionReached()){
- if(goalOrientationReached()){
- if(tc_->stopped())
- return robot_actions::SUCCESS;
- }
- else{
- //compute the velocity command we need to send for rotating to the goal
- robot_msgs::PoseDot cmd_vel;
- rotateToGoal(cmd_vel);
- publishFootprint();
- ros_node_.publish("cmd_vel", cmd_vel);
- }
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+
+ //check that the observation buffers for the costmap are current
+ if(!controller_cost_map_ros_->isCurrent()){
+ ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
+ continue;
}
- else {
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
- //check that the observation buffers for the costmap are current
- if(!controller_cost_map_ros_->isCurrent()){
- ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
- continue;
- }
+ bool valid_control = false;
+ robot_msgs::PoseDot cmd_vel;
+ std::vector<robot_actions::Pose2D> local_plan;
+ //pass plan to controller
+ lock_.lock();
+ if(valid_plan_){
+ //get observations for the non-costmap controllers
+ std::vector<Observation> observations;
+ controller_cost_map_ros_->getMarkingObservations(observations);
+ valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
+ ros_node_.publish("cmd_vel", cmd_vel);
+ }
+ lock_.unlock();
- bool valid_control = false;
- robot_msgs::PoseDot cmd_vel;
- std::vector<robot_actions::Pose2D> local_plan;
- //pass plan to controller
- lock_.lock();
- if(valid_plan_){
- //get observations for the non-costmap controllers
- std::vector<Observation> observations;
- controller_cost_map_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
- ros_node_.publish("cmd_vel", cmd_vel);
- }
- lock_.unlock();
+ //check for success
+ if(tc_->goalReached())
+ return robot_actions::SUCCESS;
- //if we don't have a valid control... we need to re-plan explicitly
- if(!valid_control){
- makePlan(goal_);
- }
- //for visualization purposes
- publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
- publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
- publishFootprint();
-
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.vel.vz);
+ //if we don't have a valid control... we need to re-plan explicitly
+ if(!valid_control){
+ makePlan(goal_);
}
+
+ //for visualization purposes
+ publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
+ publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
+ publishFootprint();
+
+ gettimeofday(&end, NULL);
+ start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+ end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+ t_diff = end_t - start_t;
+ ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.vel.vz);
+
//sleep the remainder of the cycle
if(!sleepLeftover(start_time, cycle_time))
ROS_WARN("Controll loop missed its desired cycle time of %.4f", cycle_time.toSec());
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-17 21:39:55 UTC (rev 14032)
@@ -63,6 +63,8 @@
#include <string>
+#include <angles/angles.h>
+
namespace base_local_planner {
/**
* @class TrajectoryPlannerROS
@@ -118,16 +120,58 @@
void getLocalGoal(double& x, double& y);
/**
+ * @brief Check if the goal pose has been achieved
+ * @return True if achieved, false otherwise
+ */
+ bool goalReached();
+
+ private:
+ /**
* @brief Check whether the robot is stopped or not
* @return True if the robot is stopped, false otherwise
*/
bool stopped();
+ /**
+ * @brief Check if the goal orientation has been achieved
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_x The desired x value for the goal
+ * @param goal_y The desired y value for the goal
+ * @return True if achieved, false otherwise
+ */
+ bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th);
+
+ /**
+ * @brief Check if the goal position has been achieved
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_th The desired th value for the goal
+ * @return True if achieved, false otherwise
+ */
+ bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y);
+
+ /**
+ * @brief Once a goal position is reached... rotate to the goal orientation
+ * @param global_pose The pose of the robot in the global frame
+ * @param goal_th The desired th value for the goal
+ * @param cmd_vel The velocity commands to be filled
+ */
+ void rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, double goal_th, robot_msgs::PoseDot& cmd_vel);
+
+ /**
+ * @brief Compute the distance between two points
+ * @param x1 The first x point
+ * @param y1 The first y point
+ * @param x2 The second x point
+ * @param y2 The second y point
+ * @return
+ */
+ double distance(double x1, double y1, double x2, double y2);
+
void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void tiltScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void odomCallback();
- private:
+
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
@@ -143,6 +187,8 @@
deprecated_msgs::RobotBase2DOdom odom_msg_, base_odom_; ///< @brief Used to get the velocity of the robot
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
+ double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
+ bool goal_reached_;
};
};
Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-17 21:39:55 UTC (rev 14032)
@@ -16,6 +16,7 @@
<depend package="costmap_2d" />
<depend package="robot_actions" />
<depend package="voxel_grid" />
+ <depend package="angles" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-17 21:34:39 UTC (rev 14031)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-17 21:39:55 UTC (rev 14032)
@@ -48,18 +48,21 @@
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
const Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
: world_model_(NULL), tc_(NULL), base_scan_notifier_(NULL), tf_(tf), laser_scans_(2),
- point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2){
+ point_grid_(NULL), voxel_grid_(NULL), rot_stopped_velocity_(1e-2), trans_stopped_velocity_(1e-2), goal_reached_(true){
double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
int vx_samples, vtheta_samples;
double pdist_scale, gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist;
bool holonomic_robot, dwa, simple_attractor, heading_scoring;
double heading_scoring_timestep;
- double max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th;
+ double max_vel_x, min_vel_x, max_vel_th, min_vel_th;
string world_model_type;
ros_node.param("~base_local_planner/global_frame", global_frame_, string("map"));
ros_node.param("~base_local_planner/robot_base_frame", robot_base_frame_, string("base_link"));
+ ros_node.param("~base_local_planner/yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
+ ros_node.param("~base_local_planner/xy_goal_tolerance", xy_goal_tolerance_, 0.10);
+
string odom_topic;
ros_node.param("~base_local_planner/odom_topic", odom_topic, string("odom"));
// Subscribe to odometry messages to get global pose
@@ -95,7 +98,7 @@
ros_node.param("~base_local_planner/min_vel_x", min_vel_x, 0.1);
ros_node.param("~base_local_planner/max_vel_th", max_vel_th, 1.0);
ros_node.param("~base_local_planner/min_vel_th", min_vel_th, -1.0);
- ros_node.param("~base_local_planner/min_in_place_vel_th", min_in_place_vel_th, 0.4);
+ ros_node.param("~base_local_planner/min_in_place_vel_th", min_in_place_vel_th_, 0.4);
ros_node.param("~base_local_planner/world_model", world_model_type, string("freespace"));
ros_node.param("~base_local_planner/dwa", dwa, false);
ros_node.param("~base_local_planner/heading_scoring", heading_scoring, false);
@@ -148,7 +151,7 @@
tc_ = new TrajectoryPlanner(*world_model_, cost_map, footprint_spec, inscribed_radius, circumscribed_radius,
acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, holonomic_robot,
- max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th,
+ max_vel_x, min_vel_x, max_vel_th, min_vel_th, min_in_place_vel_th_,
dwa, heading_scoring, heading_scoring_timestep, simple_attractor);
}
@@ -240,6 +243,31 @@
&& abs(base_odom_.vel.y) <= trans_stopped_velocity_;
}
+ double TrajectoryPlannerROS::distance(double x1, double y1, double x2, double y2){
+ return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
+ }
+
+ bool TrajectoryPlannerROS::goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y){
+ double dist = distance(global_pose.getOrigin().x(), global_pose.getOrigin().y(), goal_x, goal_y);
+ return fabs(dist) <= xy_goal_tolerance_;
+ }
+
+ bool TrajectoryPlannerROS::goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th){
+ double useless_pitch, useless_roll, yaw;
+ global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
+ return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance_;
+ }
+
+ void TrajectoryPlannerROS::rotateToGoal(const tf::Stamped<tf::Pose>& global_pose, double goal_th, robot_msgs::PoseDot& cmd_vel){
+ double uselessPitch, uselessRoll, yaw;
+ global_pose.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+ ROS_DEBUG("Moving to desired goal orientation\n");
+ cmd_vel.vel.vx = 0;
+ cmd_vel.vel.vy = 0;
+ double ang_diff = angles::shortest_angular_distance(yaw, goal_th);
+ cmd_vel.ang_vel.vz = ang_diff > 0.0 ? std::max(min_in_place_vel_th_, ang_diff) : std::min(-1.0 * min_in_place_vel_th_, ang_diff);
+ }
+
void TrajectoryPlannerROS::odomCallback(){
base_odom_.lock();
@@ -267,6 +295,10 @@
base_odom_.unlock();
}
+ bool TrajectoryPlannerROS::goalReached(){
+ return goal_reached_;
+ }
+
bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan,
robot_msgs::PoseDot& cmd_vel,
std::vector<robot_actions::Pose2D>& local_plan,
@@ -315,6 +347,36 @@
gettimeofday(&start, NULL);
*/
+ //we assume the global goal is the last point in the global plan
+ double goal_x = global_plan.back().x;
+ double goal_y = global_plan.back().y;
+ double goal_th = global_plan.back().th;
+
+ //assume at the beginning of our control cycle that we could have a new goal
+ goal_reached_ = false;
+
+ //check to see if we've reached the goal position
+ if(goalPositionReached(global_pose, goal_x, goal_y)){
+ //check to see if the goal orientation has been reached
+ if(goalOrientationReached(global_pose, goal_th)){
+ //set the velocity command to zero
+ cmd_vel.vel.vx = 0.0;
+ cmd_vel.vel.vy = 0.0;
+ cmd_vel.ang_vel.vz = 0.0;
+
+ //make sure that we're actually stopped before returning success
+ if(stopped())
+ goal_reached_ = true;
+ }
+ else {
+ //otherwise we need to rotate to the goal... compute the next velocity we should take
+ rotateToGoal(global_pose, goal_th, cmd_vel);
+ }
+
+ //we don't actually want to run the controller when we're just rotating to goal
+ return true;
+ }
+
tc_->updatePlan(global_plan);
obs_lock_.lock();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-18 00:36:43
|
Revision: 14048
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14048&view=rev
Author: tfoote
Date: 2009-04-18 00:36:41 +0000 (Sat, 18 Apr 2009)
Log Message:
-----------
moving CvBridge.h from image_msgs to opencv_latest
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/manifest.xml
pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_grab.cpp
pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_node.cpp
pkg/trunk/image_msgs/manifest.xml
pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_calibration.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
pkg/trunk/vision/color_calib/calib_node_dcam.cpp
pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp
pkg/trunk/vision/image_view/image_view.cpp
pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
pkg/trunk/vision/led_detection/src/led_detector.cpp
pkg/trunk/vision/outlet_detection/include/outlet_detection/plug_tracker.h
pkg/trunk/vision/outlet_detection/src/outlet_node.cpp
pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
pkg/trunk/vision/people/src/track_starter_gui.cpp
pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
pkg/trunk/vision/stereo_view/stereo_view.cpp
pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
Added Paths:
-----------
pkg/trunk/3rdparty/opencv_latest/include/
pkg/trunk/3rdparty/opencv_latest/include/opencv_latest/
pkg/trunk/3rdparty/opencv_latest/include/opencv_latest/CvBridge.h
Copied: pkg/trunk/3rdparty/opencv_latest/include/opencv_latest/CvBridge.h (from rev 10561, pkg/trunk/image_msgs/include/image_msgs/CvBridge.h)
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/include/opencv_latest/CvBridge.h (rev 0)
+++ pkg/trunk/3rdparty/opencv_latest/include/opencv_latest/CvBridge.h 2009-04-18 00:36:41 UTC (rev 14048)
@@ -0,0 +1,158 @@
+/*********************************************************************
+* 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 CVBRIDGE_HH
+#define CVBRIDGE_HH
+
+#include "image_msgs/Image.h"
+#include "opencv/cxcore.h"
+#include "opencv/cv.h"
+
+namespace image_msgs
+{
+
+ class CvBridge
+ {
+ IplImage* img_;
+ IplImage* rosimg_;
+ IplImage* cvtimg_;
+
+ void reallocIfNeeded_(IplImage** img, CvSize sz, int depth, int channels)
+ {
+ if ((*img) != 0)
+ if ((*img)->width != sz.width ||
+ (*img)->height != sz.height ||
+ (*img)->depth != depth ||
+ (*img)->nChannels != channels)
+ {
+ cvReleaseImage(img);
+ *img = 0;
+ }
+
+ if (*img == 0)
+ {
+ *img = cvCreateImage(sz, depth, channels);
+ }
+ }
+
+ public:
+
+ CvBridge() : img_(0), rosimg_(0), cvtimg_(0)
+ {
+ rosimg_ = cvCreateImageHeader( cvSize(0,0), IPL_DEPTH_8U, 1 );
+ }
+
+ ~CvBridge()
+ {
+ if (rosimg_) {
+ cvReleaseImageHeader(&rosimg_);
+ rosimg_ = 0;
+ }
+
+ if (cvtimg_) {
+ cvReleaseImage(&cvtimg_);
+ cvtimg_ = 0;
+ }
+ }
+
+
+ inline IplImage* toIpl()
+ {
+ return img_;
+ }
+
+ bool fromImage(Image& rosimg, std::string encoding = "")
+ {
+ if (rosimg.depth == "uint8")
+ {
+ cvInitImageHeader(rosimg_, cvSize(rosimg.uint8_data.layout.dim[1].size, rosimg.uint8_data.layout.dim[0].size),
+ IPL_DEPTH_8U, rosimg.uint8_data.layout.dim[2].size);
+ cvSetData(rosimg_, &(rosimg.uint8_data.data[0]), rosimg.uint8_data.layout.dim[1].stride);
+ img_ = rosimg_;
+ } else if (rosimg.depth == "uint16") {
+ cvInitImageHeader(rosimg_, cvSize(rosimg.uint16_data.layout.dim[1].size, rosimg.uint16_data.layout.dim[0].size),
+ IPL_DEPTH_16U, rosimg.uint16_data.layout.dim[2].size);
+ cvSetData(rosimg_, &(rosimg.uint16_data.data[0]), rosimg.uint16_data.layout.dim[1].stride*sizeof(uint16_t));
+ img_ = rosimg_;
+ } else {
+ return false;
+ }
+
+ if (encoding != "" && (encoding != rosimg.encoding))
+ {
+ if (encoding == "bgr" && rosimg.encoding == "rgb")
+ {
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 3);
+ cvCvtColor(rosimg_, cvtimg_, CV_RGB2BGR);
+ img_ = cvtimg_;
+ }
+ else if (encoding == "bgr" && rosimg.encoding == "mono" )
+ {
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 3);
+ cvCvtColor(rosimg_, cvtimg_, CV_GRAY2BGR);
+ img_ = cvtimg_;
+ }
+ else if (encoding == "mono" && rosimg.encoding == "rgb" )
+ {
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 1);
+ cvCvtColor(rosimg_, cvtimg_, CV_RGB2GRAY);
+ img_ = cvtimg_;
+ }
+ else if (encoding == "mono" && rosimg.encoding == "bgr" )
+ {
+ reallocIfNeeded(&cvtimg_, IPL_DEPTH_8U, 1);
+ cvCvtColor(rosimg_, cvtimg_, CV_BGR2GRAY);
+ img_ = cvtimg_;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void reallocIfNeeded(IplImage** img, int depth = -1, int channels = -1)
+ {
+ if (depth == -1)
+ depth = img_->depth;
+ if (channels == -1)
+ channels = img_->nChannels;
+ reallocIfNeeded_(img, cvGetSize(img_), depth, channels);
+ }
+ };
+}
+
+
+#endif
Modified: pkg/trunk/3rdparty/opencv_latest/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/manifest.xml 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/3rdparty/opencv_latest/manifest.xml 2009-04-18 00:36:41 UTC (rev 14048)
@@ -12,9 +12,10 @@
<review status="3rdparty" notes=""/>
<url>http://opencvlibrary.sourceforge.net</url>
<export>
- <cpp cflags="-I${prefix}/opencv/include -I${prefix}/opencv/include/opencv" lflags="-L${prefix}/opencv/lib -Wl,-rpath,${prefix}/opencv/lib -lcvaux -lcv -lcxcore -lhighgui -lml"/>
+ <cpp cflags="-I${prefix}/opencv/include -I${prefix}/opencv/include/opencv -I${prefix}/include" lflags="-L${prefix}/opencv/lib -Wl,-rpath,${prefix}/opencv/lib -lcvaux -lcv -lcxcore -lhighgui -lml"/>
<doxymaker external="http://opencvlibrary.sourceforge.net/" />
</export>
+<depend package="image_msgs" />
<sysdepend os="ubuntu" version="7.04-feisty" package="libavformat-dev"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="libavformat-dev"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="libavcodec-dev"/>
Modified: pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp
===================================================================
--- pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/drivers/cam/camera_trigger_test/src/trigger_test.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -34,7 +34,7 @@
#include "ros/node.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include <stdio.h>
#include <signal.h>
#include <robot_mechanism_controllers/SetWaveform.h>
Modified: pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_grab.cpp
===================================================================
--- pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_grab.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_grab.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -1,5 +1,5 @@
#include "ros/node.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cv.h"
#include "opencv/highgui.h"
Modified: pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_node.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/drivers/cam/prosilica_cam/src/nodes/prosilica_node.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -38,7 +38,7 @@
#include <image_msgs/Image.h>
#include <image_msgs/CamInfo.h>
#include <image_msgs/FillImage.h>
-#include <image_msgs/CvBridge.h>
+#include <opencv_latest/CvBridge.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <cv.h>
Modified: pkg/trunk/image_msgs/manifest.xml
===================================================================
--- pkg/trunk/image_msgs/manifest.xml 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/image_msgs/manifest.xml 2009-04-18 00:36:41 UTC (rev 14048)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="std_msgs"/>
- <depend package="opencv_latest"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp"/>
</export>
Modified: pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -42,7 +42,7 @@
#include <iomanip>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp
===================================================================
--- pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/mapping/door_stereo_detector/src/door_stereo_detector.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -53,7 +53,7 @@
#include <robot_msgs/VisualizationMarker.h>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/StereoInfo.h"
#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
Modified: pkg/trunk/vision/checkerboard_detector/checkerboard_calibration.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_calibration.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_calibration.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -31,7 +31,7 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/Image.h"
#include "math.h"
Modified: pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -31,7 +31,7 @@
#include "opencv/cv.h"
#include "opencv/highgui.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
#include "checkerboard_detector/ObjectDetection.h"
Modified: pkg/trunk/vision/color_calib/calib_node_dcam.cpp
===================================================================
--- pkg/trunk/vision/color_calib/calib_node_dcam.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/color_calib/calib_node_dcam.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -41,7 +41,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include <sys/stat.h>
Modified: pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp
===================================================================
--- pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/cv_mech_turk/src/cv_mturk.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -11,7 +11,7 @@
#include "opencv/highgui.h"
#include "ros/node.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include <stdlib.h>
using namespace std;
Modified: pkg/trunk/vision/image_view/image_view.cpp
===================================================================
--- pkg/trunk/vision/image_view/image_view.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/image_view/image_view.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
class ImageView
{
Modified: pkg/trunk/vision/led_detection/include/led_detection/led_detector.h
===================================================================
--- pkg/trunk/vision/led_detection/include/led_detection/led_detector.h 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/led_detection/include/led_detection/led_detector.h 2009-04-18 00:36:41 UTC (rev 14048)
@@ -38,7 +38,7 @@
#define LED_DETECTION_LED_DETECTOR_H_
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/Image.h"
#include "image_msgs/CamInfo.h"
Modified: pkg/trunk/vision/led_detection/src/led_detector.cpp
===================================================================
--- pkg/trunk/vision/led_detection/src/led_detector.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/led_detection/src/led_detector.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -67,7 +67,7 @@
/**
* Converts an openCV IPL image into a ros image message
- * \todo Robustify and move this function into image_msgs/CvBridge.h
+ * \todo Robustify and move this function into opencv_latest/CvBridge.h
* \param cv_image input: The IPL image to be converted
* \param ros_image output: The location of the destination image
*/
Modified: pkg/trunk/vision/outlet_detection/include/outlet_detection/plug_tracker.h
===================================================================
--- pkg/trunk/vision/outlet_detection/include/outlet_detection/plug_tracker.h 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/outlet_detection/include/outlet_detection/plug_tracker.h 2009-04-18 00:36:41 UTC (rev 14048)
@@ -1,7 +1,7 @@
#include <ros/node.h>
#include <image_msgs/Image.h>
#include <image_msgs/CamInfo.h>
-#include <image_msgs/CvBridge.h>
+#include <opencv_latest/CvBridge.h>
#include <robot_msgs/PoseStamped.h>
#include <prosilica_cam/PolledImage.h>
#include <tf/transform_broadcaster.h>
Modified: pkg/trunk/vision/outlet_detection/src/outlet_node.cpp
===================================================================
--- pkg/trunk/vision/outlet_detection/src/outlet_node.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/outlet_detection/src/outlet_node.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -1,7 +1,7 @@
#include <ros/node.h>
#include <image_msgs/Image.h>
#include <image_msgs/CamInfo.h>
-#include <image_msgs/CvBridge.h>
+#include <opencv_latest/CvBridge.h>
#include <robot_msgs/PoseStamped.h>
#include <prosilica_cam/PolledImage.h>
#include <tf/transform_broadcaster.h>
Modified: pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp
===================================================================
--- pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/outlet_detection/src/outlet_spotting.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -41,7 +41,7 @@
#include <string>
#include <limits>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
// opencv
#include "opencv/cxcore.h"
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -47,7 +47,7 @@
#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/ColoredLine.h"
#include "image_msgs/ColoredLines.h"
#include "topic_synchronizer/topic_synchronizer.h"
Modified: pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp
===================================================================
--- pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/people/src/stereo_face_color_tracker.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -47,7 +47,7 @@
#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "image_msgs/ColoredLines.h"
#include "topic_synchronizer/topic_synchronizer.h"
#include "tf/transform_listener.h"
Modified: pkg/trunk/vision/people/src/track_starter_gui.cpp
===================================================================
--- pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/people/src/track_starter_gui.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -45,7 +45,7 @@
#include "image_msgs/DisparityInfo.h"
#include "image_msgs/CamInfo.h"
#include "image_msgs/Image.h"
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "CvStereoCamModel.h"
#include <robot_msgs/PositionMeasurement.h>
#include "color_calib.h"
Modified: pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/recognition_lambertian/src/publish_scene.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -48,7 +48,7 @@
#include <robot_msgs/PointCloud.h>
#include <image_msgs/Image.h>
-#include <image_msgs/CvBridge.h>
+#include <opencv_latest/CvBridge.h>
#include <image_msgs/FillImage.h>
#include <std_msgs/String.h>
Modified: pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp
===================================================================
--- pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/recognition_lambertian/src/recognition_lambertian.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -43,7 +43,7 @@
#include <queue>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp
===================================================================
--- pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/stereo_capture/src/stereo_capture.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -40,7 +40,7 @@
#include <iomanip>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -34,7 +34,7 @@
#include <vector>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
Modified: pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-18 00:29:49 UTC (rev 14047)
+++ pkg/trunk/vision/stereo_view/stereo_view_pixel_info.cpp 2009-04-18 00:36:41 UTC (rev 14048)
@@ -43,7 +43,7 @@
#include <vector>
-#include "image_msgs/CvBridge.h"
+#include "opencv_latest/CvBridge.h"
#include "opencv/cxcore.h"
#include "opencv/cv.h"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-20 22:08:15
|
Revision: 14091
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14091&view=rev
Author: tfoote
Date: 2009-04-20 22:08:06 +0000 (Mon, 20 Apr 2009)
Log Message:
-----------
filling visualization_tools stack
Added Paths:
-----------
pkg/trunk/visualization_tools/Cg/
pkg/trunk/visualization_tools/freeimage/
pkg/trunk/visualization_tools/ogre/
pkg/trunk/visualization_tools/ogre_tools/
Removed Paths:
-------------
pkg/trunk/3rdparty/Cg/
pkg/trunk/3rdparty/freeimage/
pkg/trunk/3rdparty/ogre/
pkg/trunk/visualization/ogre_tools/
Property changes on: pkg/trunk/visualization_tools/Cg
___________________________________________________________________
Added: svn:ignore
+ Cg
Cg-2.0_May2008_x86.tgz
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/visualization_tools/freeimage
___________________________________________________________________
Added: svn:ignore
+ installed
freeimage-3.10.0
freeimage
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/visualization_tools/ogre
___________________________________________________________________
Added: svn:ignore
+ ogre-v1-*
ogre
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-20 23:55:22
|
Revision: 14102
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14102&view=rev
Author: hsujohnhsu
Date: 2009-04-20 23:55:17 +0000 (Mon, 20 Apr 2009)
Log Message:
-----------
update gazebo launch scripts so controllers are not loaded by default
Modified Paths:
--------------
pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_gazebo/pr2_obs_trajectory.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.xml
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.xml
pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.xml
Removed Paths:
-------------
pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory_no_x.launch
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,10 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -14,10 +14,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,9 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,10 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs_trajectory.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs_trajectory.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs_trajectory.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,10 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers_trajectory.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_simple.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -13,10 +13,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_simple_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple_no_x.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_simple_no_x.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -13,10 +13,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Deleted: pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -1,23 +0,0 @@
-<launch>
- <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/pr2_defs/robots for full pr2 -->
- <group name="wg">
- <!-- send pr2.xml to param server -->
- <param name="/use_sim_time" value="true"/>
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
-
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
-
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
-
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers_trajectory.launch" />
- </group>
-</launch>
-
Deleted: pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory_no_x.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_simple_trajectory_no_x.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -1,23 +0,0 @@
-<launch>
- <!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/pr2/pr2_defs/robots for full pr2 -->
- <group name="wg">
- <!-- send pr2.xml to param server -->
- <param name="/use_sim_time" value="true"/>
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
-
- <!-- start gazebo -->
- <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
- <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(optenv LD_LIBRARY_PATH)" />
- <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
- <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
- </node>
-
- <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
-
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers_trajectory.launch" />
- </group>
-</launch>
-
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,10 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 90" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
</group>
</launch>
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2009-04-20 23:55:17 UTC (rev 14102)
@@ -12,14 +12,8 @@
</node>
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <!-- load default arm controller -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -5 -15 0 0 90" respawn="false" output="screen" />
- <!-- load controllers -->
- <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
-
- <!-- visualization -->
- <node pkg="rviz" type="rviz" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_mechanism.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -2,6 +2,10 @@
<!-- Bring up the node we want to test. -->
<include file="$(find pr2_gazebo)/pr2_empty_no_x.launch"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
+
<!-- Run hztest -->
<!-- Test for publication of 'base_pose_ground_truth' topic. -->
<test test-name="hztest_test_base_pose_ground_truth" pkg="rostest" type="hztest" time-limit="50" name="base_pose_ground_truth_test">
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -2,6 +2,9 @@
<!-- Bring up the node we want to test. -->
<include file="$(find pr2_gazebo)/pr2_empty_no_x.launch"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
<!-- Test for publication of 'odom' topic. -->
<test test-name="hztest_test_odom" pkg="rostest" type="hztest" time-limit="50" name="odom_test">
<!-- Note how we use a different parameter namespace and node name
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_rostime.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -2,6 +2,9 @@
<!-- Bring up the node we want to test. -->
<include file="$(find pr2_gazebo)/pr2_empty_no_x.launch"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
<!-- Test for publication of 'odom' topic. -->
<test test-name="hztest_test_rostime" pkg="rostest" type="hztest" time-limit="50" name="rostime_test">
<!-- Note how we use a different parameter namespace and node name
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -1,13 +1,14 @@
<launch>
- <master auto="start" />
+ <!-- send single_link.xml to param server -->
+ <include file="$(find pr2_gazebo)/pr2_empty_no_x.launch" />
- <!-- send single_link.xml to param server -->
- <include file="$(find pr2_gazebo)/pr2_empty_no_x.launch" />
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_vw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_vw_gt.py" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_odomxyw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxyw_gt.py" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_odomxy1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxy_gt.py" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_odomw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomw_gt.py" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_odomx1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomx_gt.py" />
- <test test-name="test_pr2_mechanism_gazebo_test_base_odomy1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomy_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_vw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_vw_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_odomxyw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxyw_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_odomxy1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomxy_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_odomw1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomw_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_odomx1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomx_gt.py" />
+ <test test-name="test_pr2_mechanism_gazebo_test_base_odomy1" pkg="test_pr2_mechanism_controllers_gazebo" type="test_base_odomy_gt.py" />
</launch>
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_image.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -2,6 +2,9 @@
<!-- Bring up the node we want to test. -->
<include file="$(find pr2_gazebo)/pr2_empty.launch"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
<!-- Run hztest -->
<!-- Test for publication of 'left axis camera image publish rate' topic -->
<test test-name="hztest_test_axis_left_image" pkg="rostest" type="hztest" time-limit="60" name="axis_left_image_test">
Modified: pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.xml 2009-04-20 23:54:04 UTC (rev 14101)
+++ pkg/trunk/drivers/simulator/test_pr2_sensors_gazebo/hztest_pr2_scan.xml 2009-04-20 23:55:17 UTC (rev 14102)
@@ -2,6 +2,9 @@
<!-- Bring up the node we want to test. -->
<include file="$(find pr2_gazebo)/pr2_empty_no_x.launch"/>
+ <!-- load controllers -->
+ <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
+
<!-- Run hztest -->
<!-- Test for publication of 'base_scan' topic -->
<test test-name="hztest_test_scan" pkg="rostest" type="hztest" time-limit="50" name="base_scan_test">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-21 00:38:52
|
Revision: 14107
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14107&view=rev
Author: tfoote
Date: 2009-04-21 00:38:49 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
moving one demo/test script in to different test package and breaking dependency
Modified Paths:
--------------
pkg/trunk/demos/2dnav_stage/manifest.xml
Added Paths:
-----------
pkg/trunk/highlevel/test_highlevel_controllers/2dnav_stage_movebase_empty_room.launch
Removed Paths:
-------------
pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
Deleted: pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-04-21 00:30:40 UTC (rev 14106)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-04-21 00:38:49 UTC (rev 14107)
@@ -1,63 +0,0 @@
-<launch>
-
- <group name="wg">
- <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" output="screen"/>
- <param name="robot_radius" value="0.325"/>
- <!-- node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node -->
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
-
- <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
- <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
- <param name="move_base/plannerType" value="ARAPlanner"/>
- <param name="move_base/environmentType" value="2D"/>
- <param name="move_base/controller_frequency" value="30.0"/>
- <param name="move_base/planner_frequency" value="0.0"/>
- <param name="move_base/plannerTimeLimit" value="0.5"/>
- <param name="move_base/map_update_frequency" value="5.0"/>
- <param name="/costmap_2d/base_laser_max_range" value="20.0"/>
- <param name="/costmap_2d/tilt_laser_max_range" value="2.0"/>
- <param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
- <param name="/costmap_2d/no_information_value" value="255"/>
- <param name="/costmap_2d/z_threshold_max" value="2.0"/>
- <param name="/costmap_2d/z_threshold_min" value="0.13"/>
- <param name="/costmap_2d/freespace_projection_height" value="2.0"/>
- <param name="/costmap_2d/inflation_radius" value="0.65"/>
- <param name="/costmap_2d/circumscribed_radius" value="0.46"/>
- <param name="/costmap_2d/inscribed_radius" value="0.325"/>
- <param name="/costmap_2d/raytrace_window" value="10.0"/>
- <param name="/costmap_2d/weight" value="0.1"/>
-
- <param name="/global_frame_id" value="odom"/>
-
- <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 10 Hz -->
- <param name="/costmap_2d/base_laser_update_rate" value="2.0"/>
-
- <!-- Don't Forces a check that we are receiving tilt scans that are correctly up to date at a rate of at least 10 Hz -->
- <param name="/costmap_2d/tilt_laser_update_rate" value="0.0"/>
-
- <!-- we don't have stereo data yet -->
- <param name="/costmap_2d/stereo_update_rate" value="0.0"/>
-
- <!--- Don't check for ground plane obstacles -->
- <param name="/costmap_2d/low_obstacle_update_rate" value="0.0"/>
-
- <param name="/trajectory_rollout/map_size" value="4.0" />
- <param name="/trajectory_rollout/sim_time" value="1.0" />
- <param name="/trajectory_rollout/sim_steps" value="20" />
- <param name="/trajectory_rollout/samples_per_dim" value="25" />
- <param name="/trajectory_rollout/occdist_scale" value="0.2" />
- <param name="/trajectory_rollout/path_distance_bias" value="0.6" />
- <param name="/trajectory_rollout/goal_distance_bias" value="0.8" />
- <param name="/trajectory_rollout/acc_limit_x" value="2.5" />
- <param name="/trajectory_rollout/acc_limit_y" value="2.5" />
- <param name="/trajectory_rollout/acc_limit_th" value="3.2" />
-
- <node pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen"/>
-
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 00:30:40 UTC (rev 14106)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 00:38:49 UTC (rev 14107)
@@ -13,5 +13,4 @@
<depend package="stage"/>
<depend package="map_server"/>
<depend package="wavefront_player"/>
- <depend package="highlevel_controllers"/>
</package>
Copied: pkg/trunk/highlevel/test_highlevel_controllers/2dnav_stage_movebase_empty_room.launch (from rev 13027, pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch)
===================================================================
--- pkg/trunk/highlevel/test_highlevel_controllers/2dnav_stage_movebase_empty_room.launch (rev 0)
+++ pkg/trunk/highlevel/test_highlevel_controllers/2dnav_stage_movebase_empty_room.launch 2009-04-21 00:38:49 UTC (rev 14107)
@@ -0,0 +1,63 @@
+<launch>
+
+ <group name="wg">
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" output="screen"/>
+ <param name="robot_radius" value="0.325"/>
+ <!-- node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node -->
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+
+ <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
+ <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
+ <param name="move_base/plannerType" value="ARAPlanner"/>
+ <param name="move_base/environmentType" value="2D"/>
+ <param name="move_base/controller_frequency" value="30.0"/>
+ <param name="move_base/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="move_base/map_update_frequency" value="5.0"/>
+ <param name="/costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="/costmap_2d/tilt_laser_max_range" value="2.0"/>
+ <param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="/costmap_2d/no_information_value" value="255"/>
+ <param name="/costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="/costmap_2d/z_threshold_min" value="0.13"/>
+ <param name="/costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="/costmap_2d/inflation_radius" value="0.65"/>
+ <param name="/costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="/costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="/costmap_2d/raytrace_window" value="10.0"/>
+ <param name="/costmap_2d/weight" value="0.1"/>
+
+ <param name="/global_frame_id" value="odom"/>
+
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/base_laser_update_rate" value="2.0"/>
+
+ <!-- Don't Forces a check that we are receiving tilt scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/tilt_laser_update_rate" value="0.0"/>
+
+ <!-- we don't have stereo data yet -->
+ <param name="/costmap_2d/stereo_update_rate" value="0.0"/>
+
+ <!--- Don't check for ground plane obstacles -->
+ <param name="/costmap_2d/low_obstacle_update_rate" value="0.0"/>
+
+ <param name="/trajectory_rollout/map_size" value="4.0" />
+ <param name="/trajectory_rollout/sim_time" value="1.0" />
+ <param name="/trajectory_rollout/sim_steps" value="20" />
+ <param name="/trajectory_rollout/samples_per_dim" value="25" />
+ <param name="/trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="/trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="/trajectory_rollout/goal_distance_bias" value="0.8" />
+ <param name="/trajectory_rollout/acc_limit_x" value="2.5" />
+ <param name="/trajectory_rollout/acc_limit_y" value="2.5" />
+ <param name="/trajectory_rollout/acc_limit_th" value="3.2" />
+
+ <node pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen"/>
+
+ </group>
+</launch>
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-21 00:55:10
|
Revision: 14111
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14111&view=rev
Author: tfoote
Date: 2009-04-21 00:55:08 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
populating simulators stack
Added Paths:
-----------
pkg/trunk/simulators/gazebo/
pkg/trunk/simulators/opende/
pkg/trunk/simulators/stage/
Removed Paths:
-------------
pkg/trunk/3rdparty/gazebo/
pkg/trunk/3rdparty/opende/
pkg/trunk/3rdparty/stage/
Property changes on: pkg/trunk/simulators/gazebo
___________________________________________________________________
Added: svn:ignore
+ gazebo
patched
rospack_nosubdirs
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/simulators/opende
___________________________________________________________________
Added: svn:ignore
+ opende
installed
patched
rospack_nosubdirs
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/simulators/stage
___________________________________________________________________
Added: svn:ignore
+ installed
stage
patched
rospack_nosubdirs
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-04-21 00:57:49
|
Revision: 14112
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14112&view=rev
Author: vijaypradeep
Date: 2009-04-21 00:57:39 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
Changing stereo frame_id from stereo_link to stereo_optical_frame. stereo_image_proc now builds point clouds in a z-forward frame, instead of an x-forward frame
Modified Paths:
--------------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-04-21 00:55:08 UTC (rev 14111)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch 2009-04-21 00:57:39 UTC (rev 14112)
@@ -51,6 +51,7 @@
<param name="do_colorize" type="bool" value="False"/>
<param name="do_rectify" type="bool" value="False"/>
<param name="do_stereo" type="bool" value="False"/>
+ <param name="frame_id" type="string" value="stereo_optical_frame"/>
</node>
-->
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-04-21 00:55:08 UTC (rev 14111)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-04-21 00:57:39 UTC (rev 14112)
@@ -51,7 +51,7 @@
<param name="do_stereo" type="bool" value="False"/>
<param name="do_calc_points" type="bool" value="False"/>
<param name="fps" type="double" value="15.0"/>
- <param name="frame_id" type="string" value="stereo_link"/>
+ <param name="frame_id" type="string" value="stereo_optical_frame"/>
</group>
<node machine="three" pkg="dcam" type="stereodcam" respawn="false"/>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-04-21 00:55:08 UTC (rev 14111)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prg.launch 2009-04-21 00:57:39 UTC (rev 14112)
@@ -60,7 +60,7 @@
<param name="do_stereo" type="bool" value="False"/>
<param name="do_calc_points" type="bool" value="False"/>
<param name="fps" type="double" value="30.0"/>
- <param name="frame_id" type="string" value="stereo_link"/>
+ <param name="frame_id" type="string" value="stereo_optical_frame"/>
</group>
<node machine="four" pkg="dcam" type="stereodcam" respawn="false"/>
Modified: pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp
===================================================================
--- pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-04-21 00:55:08 UTC (rev 14111)
+++ pkg/trunk/vision/stereo_image_proc/src/stereoproc.cpp 2009-04-21 00:57:39 UTC (rev 14112)
@@ -220,9 +220,9 @@
for (int i = 0; i < stdata_->numPts; i++)
{
- cloud_.pts[i].y = -stdata_->imPts[3*i + 0];
- cloud_.pts[i].z = -stdata_->imPts[3*i + 1];
- cloud_.pts[i].x = stdata_->imPts[3*i + 2];
+ cloud_.pts[i].x = stdata_->imPts[3*i + 0];
+ cloud_.pts[i].y = stdata_->imPts[3*i + 1];
+ cloud_.pts[i].z = stdata_->imPts[3*i + 2];
}
for (int i = 0; i < stdata_->numPts; i++)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-21 02:00:45
|
Revision: 14117
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14117&view=rev
Author: tfoote
Date: 2009-04-21 02:00:43 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
moving map_server into nav stack
Added Paths:
-----------
pkg/trunk/nav/map_server/
Removed Paths:
-------------
pkg/trunk/world_models/map_server/
Property changes on: pkg/trunk/nav/map_server
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-21 02:18:53
|
Revision: 14123
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14123&view=rev
Author: eitanme
Date: 2009-04-21 02:18:49 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
The new navigation stack now uses PoseStamped instead of Pose2D
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/manifest.xml
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/manifest.xml
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/nav/nav_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/src/nav_view/tools.cpp
pkg/trunk/visualization/rviz/manifest.xml
pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-21 02:18:49 UTC (rev 14123)
@@ -38,8 +38,8 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <robot_actions/MoveBaseState.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_actions/MoveBaseStateNew.h>
+#include <robot_msgs/PoseStamped.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
@@ -53,7 +53,7 @@
* @class MoveBaseAction
* @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
*/
- class MoveBaseAction : public robot_actions::Action<robot_actions::Pose2D, robot_actions::Pose2D> {
+ class MoveBaseAction : public robot_actions::Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped> {
public:
/**
* @brief Constructor for the actions
@@ -74,16 +74,17 @@
* @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
* @return The result of the execution, ie: Success, Preempted, Aborted, etc.
*/
- virtual robot_actions::ResultStatus execute(const robot_actions::Pose2D& goal, robot_actions::Pose2D& feedback);
+ virtual robot_actions::ResultStatus execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback);
private:
/**
* @brief Sleeps for the remainder of a cycle
* @param start The start time of the cycle
* @param cycle_time The desired cycle time
+ * @param actual Will be set to the actual cycle time of the loop
* @return True if the desired cycle time is met, false otherwise
*/
- bool sleepLeftover(ros::Time start, ros::Duration cycle_time);
+ bool sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual);
/**
* @brief Publishes the footprint of the robot for visualization purposes
@@ -93,13 +94,13 @@
/**
* @brief Publish a path for visualization purposes
*/
- void publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a);
+ void publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
/**
* @brief Make a new global plan
* @param goal The goal to plan to
*/
- void makePlan(const robot_actions::Pose2D& goal);
+ void makePlan(const robot_msgs::PoseStamped& goal);
/**
* @brief Trim off parts of the global plan that are far enough behind the robot
@@ -130,12 +131,12 @@
costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
navfn::NavfnROS* planner_;
- std::vector<robot_actions::Pose2D> global_plan_;
+ std::vector<robot_msgs::PoseStamped> global_plan_;
std::vector<robot_msgs::Point> footprint_;
std::string global_frame_, robot_base_frame_;
bool valid_plan_;
boost::recursive_mutex lock_;
- robot_actions::Pose2D goal_;
+ robot_msgs::PoseStamped goal_;
tf::Stamped<tf::Pose> global_pose_;
double inscribed_radius_, circumscribed_radius_, inflation_radius_;
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -43,7 +43,7 @@
namespace nav {
MoveBaseAction::MoveBaseAction(ros::Node& ros_node, tf::TransformListener& tf) :
- Action<Pose2D, Pose2D>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
+ Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
run_planner_(true), tc_(NULL), planner_cost_map_ros_(NULL), controller_cost_map_ros_(NULL),
planner_(NULL), valid_plan_(false) {
@@ -151,7 +151,7 @@
}
- void MoveBaseAction::makePlan(const robot_actions::Pose2D& goal){
+ void MoveBaseAction::makePlan(const robot_msgs::PoseStamped& goal){
//since this gets called on handle activate
if(planner_cost_map_ros_ == NULL)
return;
@@ -162,7 +162,7 @@
//make sure we clear the robot's footprint from the cost map
clearRobotFootprint(planner_cost_map_);
- std::vector<robot_actions::Pose2D> global_plan;
+ std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
//we'll also push the goal point onto the end of the plan to make sure orientation is taken into account
@@ -199,15 +199,15 @@
void MoveBaseAction::prunePlan(){
lock_.lock();
- std::vector<robot_actions::Pose2D>::iterator it = global_plan_.begin();
+ std::vector<robot_msgs::PoseStamped>::iterator it = global_plan_.begin();
while(it != global_plan_.end()){
- const robot_actions::Pose2D& w = *it;
+ const robot_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- double x_diff = global_pose_.getOrigin().x() - w.x;
- double y_diff = global_pose_.getOrigin().y() - w.y;
+ double x_diff = global_pose_.getOrigin().x() - w.pose.position.x;
+ double y_diff = global_pose_.getOrigin().y() - w.pose.position.y;
double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
if(distance < 1){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.x, w.y);
+ ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.pose.position.x, w.pose.position.y);
break;
}
it = global_plan_.erase(it);
@@ -215,7 +215,7 @@
lock_.unlock();
}
- robot_actions::ResultStatus MoveBaseAction::execute(const robot_actions::Pose2D& goal, robot_actions::Pose2D& feedback){
+ robot_actions::ResultStatus MoveBaseAction::execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback){
//update the goal
goal_ = goal;
@@ -234,14 +234,7 @@
updateGlobalPose();
//update feedback to correspond to our current position
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- feedback.header.frame_id = global_frame_;
- feedback.header.stamp = ros::Time::now();
- feedback.x = global_pose_.getOrigin().x();
- feedback.y = global_pose_.getOrigin().y();
- feedback.z = 0.0;
- feedback.th = yaw;
+ tf::PoseStampedTFToMsg(global_pose_, feedback);
//push the feedback out
update(feedback);
@@ -268,7 +261,7 @@
bool valid_control = false;
robot_msgs::PoseDot cmd_vel;
- std::vector<robot_actions::Pose2D> local_plan;
+ std::vector<robot_msgs::PoseStamped> local_plan;
//pass plan to controller
lock_.lock();
if(valid_plan_){
@@ -310,18 +303,23 @@
t_diff = end_t - start_t;
ROS_DEBUG("Full control cycle: %.9f Valid control: %d, Vel Cmd (%.2f, %.2f, %.2f)", t_diff, valid_control, cmd_vel.vel.vx, cmd_vel.vel.vy, cmd_vel.ang_vel.vz);
+ ros::Duration actual;
//sleep the remainder of the cycle
- if(!sleepLeftover(start_time, cycle_time))
- ROS_WARN("Controll loop missed its desired cycle time of %.4f", cycle_time.toSec());
+ if(!sleepLeftover(start_time, cycle_time, actual))
+ ROS_WARN("Controll loop missed its desired cycle time of %.4f... the loop actually took %.4f seconds", cycle_time.toSec(), actual.toSec());
}
return robot_actions::PREEMPTED;
}
- bool MoveBaseAction::sleepLeftover(ros::Time start, ros::Duration cycle_time){
+ bool MoveBaseAction::sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual){
ros::Time expected_end = start + cycle_time;
+ ros::Time actual_end = ros::Time::now();
///@todo: because durations don't handle subtraction properly right now
- ros::Duration sleep_time = ros::Duration((expected_end - ros::Time::now()).toSec());
+ ros::Duration sleep_time = ros::Duration((expected_end - actual_end).toSec());
+ //set the actual amount of time the loop took
+ actual = actual_end - start;
+
if(sleep_time < ros::Duration(0.0)){
return false;
}
@@ -353,14 +351,19 @@
ros_node_.publish("robot_footprint", footprint_msg);
}
- void MoveBaseAction::publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
- // Extract the plan in world co-ordinates
+ void MoveBaseAction::publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
+ //given an empty path we won't do anything
+ if(path.empty())
+ return;
+
+ // Extract the plan in world co-ordinates, we assume the path is all in the same frame
robot_msgs::Polyline2D gui_path_msg;
- gui_path_msg.header.frame_id = global_frame_;
+ gui_path_msg.header.frame_id = path[0].header.frame_id;
+ gui_path_msg.header.stamp = path[0].header.stamp;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
- gui_path_msg.points[i].x = path[i].x;
- gui_path_msg.points[i].y = path[i].y;
+ gui_path_msg.points[i].x = path[i].pose.position.x;
+ gui_path_msg.points[i].y = path[i].pose.position.y;
}
gui_path_msg.color.r = r;
@@ -380,7 +383,7 @@
nav::MoveBaseAction move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
- runner.connect<Pose2D, MoveBaseState, Pose2D>(move_base);
+ runner.connect<robot_msgs::PoseStamped, MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
runner.run();
ros_node.spin();
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-21 02:18:49 UTC (rev 14123)
@@ -40,7 +40,7 @@
#include <ros/node.h>
#include <navfn/navfn.h>
#include <costmap_2d/costmap_2d.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
#include <tf/transform_listener.h>
#include <vector>
@@ -63,7 +63,7 @@
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
- bool makePlan(const robot_actions::Pose2D& goal, std::vector<robot_actions::Pose2D>& plan);
+ bool makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan);
/**
* @brief Compute the full navigation function for the costmap given a point in the world to start from
Modified: pkg/trunk/motion_planning/navfn/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/navfn/manifest.xml 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/manifest.xml 2009-04-21 02:18:49 UTC (rev 14123)
@@ -7,7 +7,6 @@
<url>http://pr.willowgarage.com</url>
<depend package="rosconsole"/>
<depend package="roscpp"/>
- <depend package="robot_actions"/>
<depend package="robot_msgs"/>
<depend package="costmap_2d"/>
<depend package="tf"/>
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -74,15 +74,21 @@
return planner_.calcNavFnDijkstra();
}
- bool NavfnROS::makePlan(const robot_actions::Pose2D& goal, std::vector<robot_actions::Pose2D>& plan){
+ bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
//get the pose of the robot in the global frame
- tf::Stamped<tf::Pose> robot_pose, global_pose;
+ tf::Stamped<tf::Pose> robot_pose, global_pose, orig_goal, goal_pose;
+
+ //convert the goal message into tf::Stamped<tf::Pose>
+ tf::PoseStampedMsgToTF(goal, orig_goal);
+
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
+ robot_pose.stamp_ = ros::Time::now();
try{
+ //transform both the goal and pose of the robot to the global frame
tf_.transformPose(global_frame_, robot_pose, global_pose);
+ tf_.transformPose(global_frame_, orig_goal, goal_pose);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -107,7 +113,10 @@
map_start[0] = mx;
map_start[1] = my;
- if(!cost_map_.worldToMap(goal.x, goal.y, mx, my))
+ wx = goal_pose.getOrigin().x();
+ wy = goal_pose.getOrigin().y();
+
+ if(!cost_map_.worldToMap(wx, wy, mx, my))
return false;
int map_goal[2];
@@ -133,9 +142,11 @@
double world_x, world_y;
cost_map_.mapToWorld(cell_x, cell_y, world_x, world_y);
- robot_actions::Pose2D pose;
- pose.x = world_x;
- pose.y = world_y;
+ robot_msgs::PoseStamped pose;
+ pose.header.stamp = ros::Time::now();
+ pose.header.frame_id = global_frame_;
+ pose.pose.position.x = world_x;
+ pose.pose.position.y = world_y;
plan.push_back(pose);
}
}
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner.h 2009-04-21 02:18:49 UTC (rev 14123)
@@ -52,7 +52,7 @@
#include <base_local_planner/trajectory.h>
//we'll take in a path as a vector of poses
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
#include <base_local_planner/Position2DInt.h>
@@ -140,7 +140,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<robot_actions::Pose2D>& new_plan);
+ void updatePlan(const std::vector<robot_msgs::PoseStamped>& new_plan);
/**
* @brief Used for display purposes, allows the footprint of the robot to be drawn in visualization tools
@@ -256,7 +256,7 @@
double inscribed_radius_, circumscribed_radius_; ///< @brief The inscribed and circumscribed radii of the robot
- std::vector<robot_actions::Pose2D> global_plan_; ///< @brief The global path for the robot to follow
+ std::vector<robot_msgs::PoseStamped> 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/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-21 02:18:49 UTC (rev 14123)
@@ -50,7 +50,7 @@
#include <tf/transform_datatypes.h>
#include <deprecated_msgs/RobotBase2DOdom.h>
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
@@ -100,15 +100,15 @@
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
- * @param global_plan The plan to pass to the controller
+ * @param orig_global_plan The plan to pass to the controller
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @param local_plan Will be set from the points of the selected trajectory for display purposes
* @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan,
+ bool computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan,
robot_msgs::PoseDot& cmd_vel,
- std::vector<robot_actions::Pose2D>& local_plan,
+ std::vector<robot_msgs::PoseStamped>& local_plan,
const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0));
/**
Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/manifest.xml 2009-04-21 02:18:49 UTC (rev 14123)
@@ -14,7 +14,6 @@
<depend package="roslib" />
<depend package="rospy" />
<depend package="costmap_2d" />
- <depend package="robot_actions" />
<depend package="voxel_grid" />
<depend package="angles" />
<url>http://pr.willowgarage.com</url>
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -39,7 +39,6 @@
using namespace std;
using namespace robot_msgs;
-using namespace robot_actions;
using namespace costmap_2d;
namespace base_local_planner{
@@ -101,8 +100,8 @@
queue<MapCell*> path_dist_queue;
queue<MapCell*> goal_dist_queue;
for(unsigned int i = 0; i < global_plan_.size(); ++i){
- double g_x = global_plan_[i].x;
- double g_y = global_plan_[i].y;
+ double g_x = global_plan_[i].pose.position.x;
+ double g_y = global_plan_[i].pose.position.y;
unsigned int map_x, map_y;
if(cost_map_.worldToMap(g_x, g_y, map_x, map_y)){
MapCell& current = map_(map_x, map_y);
@@ -295,8 +294,8 @@
//do we want to follow blindly
if(simple_attractor_){
- goal_dist = (x_i - global_plan_[global_plan_.size() -1].x) * (x_i - global_plan_[global_plan_.size() -1].x) +
- (y_i - global_plan_[global_plan_.size() -1].y) * (y_i - global_plan_[global_plan_.size() -1].y);
+ goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) * (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
+ (y_i - global_plan_[global_plan_.size() -1].pose.position.y) * (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
path_dist = 0.0;
}
else{
@@ -339,7 +338,7 @@
unsigned int goal_cell_x, goal_cell_y;
//find a clear line of sight from the robot's cell to a point on the path
for(int i = global_plan_.size() - 1; i >=0; --i){
- if(cost_map_.worldToMap(global_plan_[i].x, global_plan_[i].y, goal_cell_x, goal_cell_y)){
+ if(cost_map_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)){
if(lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0){
double gx, gy;
cost_map_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
@@ -453,7 +452,7 @@
return cost;
}
- void TrajectoryPlanner::updatePlan(const vector<Pose2D>& new_plan){
+ void TrajectoryPlanner::updatePlan(const vector<PoseStamped>& new_plan){
global_plan_.resize(new_plan.size());
for(unsigned int i = 0; i < new_plan.size(); ++i){
global_plan_[i] = new_plan[i];
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -299,9 +299,9 @@
return goal_reached_;
}
- bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_actions::Pose2D>& global_plan,
+ bool TrajectoryPlannerROS::computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan,
robot_msgs::PoseDot& cmd_vel,
- std::vector<robot_actions::Pose2D>& local_plan,
+ std::vector<robot_msgs::PoseStamped>& local_plan,
const std::vector<costmap_2d::Observation>& observations){
tf::Stamped<tf::Pose> robot_pose, global_pose;
@@ -309,9 +309,17 @@
robot_pose.frame_id_ = robot_base_frame_;
robot_pose.stamp_ = ros::Time();
+ std::vector<robot_msgs::PoseStamped> global_plan;
+
//get the global pose of the robot
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
+ //transform the plan for the robot into the global frame for the controller
+ for(unsigned int i = 0; i < orig_global_plan.size(); ++i){
+ robot_msgs::PoseStamped new_pose;
+ tf_.transformPose(global_frame_, orig_global_plan[i], new_pose);
+ global_plan.push_back(new_pose);
+ }
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -351,11 +359,17 @@
if(global_plan.empty())
return false;
+ tf::Stamped<tf::Pose> goal_point;
+ tf::PoseStampedMsgToTF(global_plan.back(), goal_point);
//we assume the global goal is the last point in the global plan
- double goal_x = global_plan.back().x;
- double goal_y = global_plan.back().y;
- double goal_th = global_plan.back().th;
+ double goal_x = goal_point.getOrigin().getX();
+ double goal_y = goal_point.getOrigin().getY();
+ double uselessPitch, uselessRoll, yaw;
+ goal_point.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
+
+ double goal_th = yaw;
+
//assume at the beginning of our control cycle that we could have a new goal
goal_reached_ = false;
@@ -399,7 +413,6 @@
//pass along drive commands
cmd_vel.vel.vx = drive_cmds.getOrigin().getX();
cmd_vel.vel.vy = drive_cmds.getOrigin().getY();
- double uselessPitch, uselessRoll, yaw;
drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
cmd_vel.ang_vel.vz = yaw;
@@ -413,11 +426,10 @@
double p_x, p_y, p_th;
path.getPoint(i, p_x, p_y, p_th);
- robot_actions::Pose2D p;
- p.x = p_x;
- p.y = p_y;
- p.th = p_th;
- local_plan.push_back(p);
+ tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(p_th, 0.0, 0.0), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_);
+ robot_msgs::PoseStamped pose;
+ tf::PoseStampedTFToMsg(p, pose);
+ local_plan.push_back(pose);
}
/* For Debugging
Modified: pkg/trunk/nav/nav_view/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_view/manifest.xml 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/manifest.xml 2009-04-21 02:18:49 UTC (rev 14123)
@@ -5,7 +5,6 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
<depend package="robot_srvs"/>
- <depend package="robot_actions"/>
<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-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -116,7 +116,7 @@
ros_node_->param("/global_frame_id", global_frame_id_, std::string("/map"));
- ros_node_->advertise<robot_actions::Pose2D>("goal", 1);
+ ros_node_->advertise<robot_msgs::PoseStamped>("goal", 1);
ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
ros_node_->subscribe("gui_path", path_line_, &NavViewPanel::incomingGuiPath, this, 1);
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-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-04-21 02:18:49 UTC (rev 14123)
@@ -33,7 +33,7 @@
#include "nav_view_panel_generated.h"
#include "robot_msgs/ParticleCloud.h"
-#include "robot_actions/Pose2D.h"
+#include "robot_msgs/PoseStamped.h"
#include "robot_msgs/Polyline2D.h"
#include "robot_msgs/PoseWithCovariance.h"
#include "robot_srvs/StaticMap.h"
@@ -95,7 +95,7 @@
- @b "inflated_obstacles"/Polyline2D : Inflated obstacle data. Rendered as points
Publishes to (name / type):
-- @b "goal"/Pose2D : goal for planner. Sent when using the Goal tool
+- @b "goal"/PoseStamped : goal for planner. Sent when using the Goal tool
- @b "initialpose"/Pose2DFloat32 : pose to initialize localization system. Sent when using the Pose tool
<hr>
@@ -207,7 +207,7 @@
Ogre::TexturePtr map_texture_;
robot_msgs::ParticleCloud cloud_;
- robot_actions::Pose2D goal_;
+ robot_msgs::PoseStamped goal_;
robot_msgs::Polyline2D path_line_;
robot_msgs::Polyline2D local_path_;
robot_msgs::Polyline2D robot_footprint_;
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -160,12 +160,10 @@
if ( is_goal_ )
{
- robot_actions::Pose2D goal;
- goal.x = pos_.x;
- goal.y = pos_.y;
- goal.th = angle;
- goal.header.frame_id = panel_->getGlobalFrame();
- printf("setting goal: %.3f %.3f %.3f\n", goal.x, goal.y, goal.th);
+ tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::Quaternion(angle, 0.0, 0.0), tf::Point(pos_.x, pos_.y, 0.0)), ros::Time::now(), panel_->getGlobalFrame());
+ robot_msgs::PoseStamped goal;
+ tf::PoseStampedTFToMsg(p, goal);
+ printf("setting goal: %.3f %.3f %.3f\n", pos_.x, pos_.y, angle);
ros_node_->publish( "goal", goal );
}
else
Modified: pkg/trunk/visualization/rviz/manifest.xml
===================================================================
--- pkg/trunk/visualization/rviz/manifest.xml 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/visualization/rviz/manifest.xml 2009-04-21 02:18:49 UTC (rev 14123)
@@ -14,7 +14,6 @@
<depend package="rosconsole"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
- <depend package="robot_actions"/>
<depend package="tf"/>
<depend package="laser_scan"/>
<depend package="gazebo_robot_description"/>
Modified: pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-21 02:18:03 UTC (rev 14122)
+++ pkg/trunk/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-04-21 02:18:49 UTC (rev 14123)
@@ -36,7 +36,7 @@
#include "ogre_tools/arrow.h"
#include "ogre_tools/wx_ogre_render_window.h"
-#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseWithCovariance.h>
#include <OGRE/OgreRay.h>
@@ -61,7 +61,7 @@
arrow_->setColor( 0.0f, 1.0f, 0.0f, 1.0f );
arrow_->getSceneNode()->setVisible( false );
- ros_node_->advertise<robot_actions::Pose2D>("goal", 1);
+ ros_node_->advertise<robot_msgs::PoseStamped>("goal", 1);
ros_node_->advertise<robot_msgs::PoseWithCovariance>("initialpose", 1);
}
@@ -153,12 +153,10 @@
if ( is_goal_ )
{
- robot_actions::Pose2D goal;
- goal.x = robot_pos_transformed.x();
- goal.y = robot_pos_transformed.y();
-...
[truncated message content] |
|
From: <tf...@us...> - 2009-04-21 04:12:46
|
Revision: 14129
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14129&view=rev
Author: tfoote
Date: 2009-04-21 04:12:37 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
moving laser_scan and image_msgs into prcore for now
Added Paths:
-----------
pkg/trunk/prcore/image_msgs/
pkg/trunk/prcore/laser_scan/
Removed Paths:
-------------
pkg/trunk/image_msgs/
pkg/trunk/util/laser_scan/
Property changes on: pkg/trunk/prcore/image_msgs
___________________________________________________________________
Added: svn:mergeinfo
+
Property changes on: pkg/trunk/prcore/laser_scan
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-04-21 04:29:51
|
Revision: 14130
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14130&view=rev
Author: gerkey
Date: 2009-04-21 04:29:39 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
Moved wavefront_player to wavefront.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
Added Paths:
-----------
pkg/trunk/nav/wavefront/
pkg/trunk/nav/wavefront/CMakeLists.txt
pkg/trunk/nav/wavefront/Makefile
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/heap.c
pkg/trunk/nav/wavefront/src/heap.h
pkg/trunk/nav/wavefront/src/plan.c
pkg/trunk/nav/wavefront/src/plan.h
pkg/trunk/nav/wavefront/src/plan_control.c
pkg/trunk/nav/wavefront/src/plan_plan.c
pkg/trunk/nav/wavefront/src/plan_waypoint.c
pkg/trunk/nav/wavefront/src/test.c
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Removed Paths:
-------------
pkg/trunk/nav/wavefront_player/
Modified: pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -2,7 +2,7 @@
<group name="wg">
<include file="$(find 2dnav_erratic)/lowlevel.xml"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen" />
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_erratic/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -8,6 +8,6 @@
<depend package="erratic_player"/>
<depend package="sicktoolbox_wrapper"/>
<depend package="map_server"/>
- <depend package="wavefront_player"/>
+ <depend package="wavefront"/>
<depend package="highlevel_controllers"/>
</package>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -12,7 +12,6 @@
<depend package="map_server"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="wavefront_player"/>
<depend package="teleop_base"/>
<depend package="teleop_arm_keyboard"/>
<depend package="highlevel_controllers"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,7 +3,7 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,7 +3,7 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
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-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,13 +3,13 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <param name="robot_0/wavefront_player_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
+ <param name="robot_0/wavefront_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
+ <node pkg="wavefront" type="wavefront" respawn="false" name="wavefront_0" ns="robot_0" output="screen">
<param name="tf_prefix" value="robot_0"/>
<remap from="scan" to="base_scan" />
</node>
- <param name="robot_1/wavefront_player_1/tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_1" ns="robot_1" >
+ <param name="robot_1/wavefront_1/tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
+ <node pkg="wavefront" type="wavefront" respawn="false" name="wavefront_1" ns="robot_1" >
<param name="tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
<remap from="scan" to="base_scan" />
</node>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,24 +3,24 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="6"/>
+ <param name="pf_laser_max_beams" value="20"/>
<param name="pf_min_samples" value="100"/>
<param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.1"/>
- <param name="pf_odom_drift_yy" value="0.1"/>
- <param name="pf_odom_drift_aa" value="0.1"/>
- <param name="pf_odom_drift_xa" value="0.1"/>
+ <param name="pf_odom_drift_xx" value="0.5"/>
+ <param name="pf_odom_drift_yy" value="0.5"/>
+ <param name="pf_odom_drift_aa" value="0.5"/>
+ <param name="pf_odom_drift_xa" value="0.5"/>
<param name="pf_min_d" value="0.25"/> <!-- 25cm -->
<param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
+ <node pkg="amcl_player" type="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+ <!--node pkg="nav_view" type="nav_view" respawn="false" output="screen"/-->
+ <node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/>
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -16,7 +16,7 @@
<remap from="scan" to="base_scan" />
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -35,7 +35,7 @@
<param name="robot_th_start" value="0.0"/>
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -12,5 +12,5 @@
<depend package="fake_localization"/>
<depend package="stage"/>
<depend package="map_server"/>
- <depend package="wavefront_player"/>
+ <depend package="wavefront"/>
</package>
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -9,7 +9,6 @@
<depend package="map_server"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="wavefront_player"/>
<depend package="teleop_arm_keyboard"/>
<depend package="highlevel_controllers"/>
<depend package="executive_trex_pr2"/>
Added: pkg/trunk/nav/wavefront/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/wavefront/CMakeLists.txt (rev 0)
+++ pkg/trunk/nav/wavefront/CMakeLists.txt 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(wavefront)
+
+rospack_add_boost_directories()
+
+rospack_add_library(wavefront_planner
+ src/heap.c
+ src/plan.c
+ src/plan_control.c
+ src/plan_plan.c
+ src/plan_waypoint.c)
+
+rospack_add_executable(bin/wavefront src/wavefront_node.cpp)
+target_link_libraries(bin/wavefront wavefront_planner)
+rospack_link_boost(bin/wavefront thread)
+
+# this little program just lets you send and block on goals from the shell
+rospack_add_executable(bin/cli src/cli.cpp)
+rospack_link_boost(bin/cli thread)
+
+# TODO: Make this program into a real unit test, and wean it off GDK
+#rospack_add_executable(bin/test src/test.c)
+#target_link_libraries(bin/test wavefront_planner)
Added: pkg/trunk/nav/wavefront/Makefile
===================================================================
--- pkg/trunk/nav/wavefront/Makefile (rev 0)
+++ pkg/trunk/nav/wavefront/Makefile 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml (rev 0)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,12 @@
+<package>
+ <description>A ROS node that uses the Player libwavefront_standalone library, which implements wavefront path-planning for a planar robot.</description>
+ <author>Brian P. Gerkey</author>
+ <license>LGPL</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="laser_scan" />
+ <depend package="robot_srvs" />
+ <depend package="robot_msgs" />
+ <depend package="robot_actions" />
+ <depend package="tf"/>
+</package>
Added: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp (rev 0)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,70 @@
+#include <cstdio>
+#include <stdlib.h>
+#include "ros/node.h"
+#include "ros/publisher.h"
+#include "robot_msgs/Planner2DState.h"
+#include "robot_msgs/Planner2DGoal.h"
+
+class WavefrontCLI : public ros::Node
+{
+public:
+ robot_msgs::Planner2DState wf_state;
+ robot_msgs::Planner2DGoal wf_goal;
+ enum { WF_IDLE, WF_SEEKING_GOAL, WF_DONE } state;
+
+ WavefrontCLI(double x, double y, double th)
+ : ros::Node("wavefront_cli"), state(WF_IDLE)
+ {
+ wf_goal.goal.x = x;
+ wf_goal.goal.y = y;
+ wf_goal.goal.th = th;
+ wf_goal.enable = 1;
+ subscribe("state", wf_state, &WavefrontCLI::state_cb, 1);
+ advertise("goal", wf_goal, &WavefrontCLI::goal_subscriber_callback, 1);
+ }
+ void state_cb()
+ {
+ if (state == WF_IDLE && wf_state.status.value == wf_state.status.ACTIVE)
+ state = WF_SEEKING_GOAL;
+ else if (state == WF_SEEKING_GOAL && wf_state.status.value != wf_state.status.ACTIVE)
+ state = WF_DONE;
+ }
+ void deactivate_goal()
+ {
+ wf_goal.enable = 0;
+ publish("goal", wf_goal);
+ ros::Duration().fromSec(0.5).sleep(); // hack to try and wait for the message to go
+ }
+ void goal_subscriber_callback(const ros::PublisherPtr& pub)
+ {
+ publish("goal", wf_goal);
+ }
+};
+
+int main(int argc, char **argv)
+{
+ if (argc != 5)
+ {
+ printf("usage: ./cli X Y THETA TIMEOUT\n"
+ " where X and Y are in meters,\n"
+ " THETA is in radians,\n"
+ " and TIMEOUT is in seconds.\n");
+ return 1;
+ }
+ ros::init(argc, argv);
+ double max_secs = atof(argv[4]);
+ WavefrontCLI wf_cli(atof(argv[1]), atof(argv[2]), atof(argv[3]));
+ ros::Time t_start(ros::Time::now());
+ while (wf_cli.ok() && wf_cli.state != WavefrontCLI::WF_DONE &&
+ ros::Time::now() - t_start < ros::Duration().fromSec(max_secs))
+ ros::Duration().fromSec(0.1).sleep();
+ if (wf_cli.ok() && wf_cli.state != WavefrontCLI::WF_DONE) // didn't get there
+ {
+ printf("timeout\n");
+ wf_cli.deactivate_goal();
+ }
+ else
+ printf("success\n");
+
+ return 0;
+}
Added: pkg/trunk/nav/wavefront/src/heap.c
===================================================================
--- pkg/trunk/nav/wavefront/src/heap.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/heap.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,166 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey ge...@wi...
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+#include <stdlib.h>
+#include <assert.h>
+#include <stdio.h>
+
+#include "heap.h"
+
+heap_t*
+heap_alloc(int size, heap_free_elt_fn_t free_fn)
+{
+ heap_t* h;
+
+ h = calloc(1,sizeof(heap_t));
+ assert(h);
+ h->size = size;
+ h->free_fn = free_fn;
+ h->A = calloc(h->size,sizeof(double));
+ assert(h->A);
+ h->data = calloc(h->size,sizeof(void*));
+ assert(h->data);
+ h->len = 0;
+
+ return(h);
+}
+
+void
+heap_free(heap_t* h)
+{
+ if(h->free_fn)
+ {
+ while(!heap_empty(h))
+ (*h->free_fn)(heap_extract_max(h));
+ }
+ free(h->data);
+ free(h->A);
+ free(h);
+}
+
+void
+heap_heapify(heap_t* h, int i)
+{
+ int l, r;
+ int largest;
+ double tmp;
+ void* tmp_data;
+
+ l = HEAP_LEFT(i);
+ r = HEAP_RIGHT(i);
+
+ if((l < h->len) && (h->A[l] > h->A[i]))
+ largest = l;
+ else
+ largest = i;
+
+ if((r < h->len) && (h->A[r] > h->A[largest]))
+ largest = r;
+
+ if(largest != i)
+ {
+ tmp = h->A[i];
+ tmp_data = h->data[i];
+ h->A[i] = h->A[largest];
+ h->data[i] = h->data[largest];
+ h->A[largest] = tmp;
+ h->data[largest] = tmp_data;
+ heap_heapify(h,largest);
+ }
+}
+
+int
+heap_empty(heap_t* h)
+{
+ return(h->len == 0);
+}
+
+void*
+heap_extract_max(heap_t* h)
+{
+ void* max;
+
+ assert(h->len > 0);
+
+ max = h->data[0];
+ h->A[0] = h->A[h->len - 1];
+ h->data[0] = h->data[h->len - 1];
+ h->len--;
+ heap_heapify(h,0);
+ return(max);
+}
+
+void
+heap_insert(heap_t* h, double key, void* data)
+{
+ int i;
+
+ if(h->len == h->size)
+ {
+ h->size *= 2;
+ h->A = realloc(h->A, h->size * sizeof(double));
+ assert(h->A);
+ h->data = realloc(h->data, h->size * sizeof(void*));
+ assert(h->data);
+ }
+
+ h->len++;
+ i = h->len - 1;
+
+ while((i > 0) && (h->A[HEAP_PARENT(i)] < key))
+ {
+ h->A[i] = h->A[HEAP_PARENT(i)];
+ h->data[i] = h->data[HEAP_PARENT(i)];
+ i = HEAP_PARENT(i);
+ }
+ h->A[i] = key;
+ h->data[i] = data;
+}
+
+int
+heap_valid(heap_t* h)
+{
+ int i;
+ for(i=1;i<h->len;i++)
+ {
+ if(h->A[HEAP_PARENT(i)] < h->A[i])
+ return(0);
+ }
+ return(1);
+}
+
+void
+heap_reset(heap_t* h)
+{
+ h->len = 0;
+}
+
+void
+heap_dump(heap_t* h)
+{
+ int i;
+ for(i=0;i<h->len;i++)
+ printf("%d: %f\n", i, h->A[i]);
+}
Added: pkg/trunk/nav/wavefront/src/heap.h
===================================================================
--- pkg/trunk/nav/wavefront/src/heap.h (rev 0)
+++ pkg/trunk/nav/wavefront/src/heap.h 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,65 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey ge...@wi...
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+
+#ifndef _HEAP_H_
+#define _HEAP_H_
+
+#define HEAP_PARENT(i) ((i)/2)
+#define HEAP_LEFT(i) (2*(i))
+#define HEAP_RIGHT(i) (2*(i)+1)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct heap;
+
+typedef void (*heap_free_elt_fn_t) (void* elt);
+
+typedef struct heap
+{
+ int len;
+ int size;
+ heap_free_elt_fn_t free_fn;
+ double* A;
+ void** data;
+} heap_t;
+
+heap_t* heap_alloc(int size, heap_free_elt_fn_t free_fn);
+void heap_free(heap_t* h);
+void heap_heapify(heap_t* h, int i);
+void* heap_extract_max(heap_t* h);
+void heap_insert(heap_t* h, double key, void* data);
+void heap_dump(heap_t* h);
+int heap_valid(heap_t* h);
+int heap_empty(heap_t* h);
+void heap_reset(heap_t* h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Added: pkg/trunk/nav/wavefront/src/plan.c
===================================================================
--- pkg/trunk/nav/wavefront/src/plan.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/plan.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,757 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Andrew Howard
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**************************************************************************
+ * Desc: Path planning
+ * Author: Andrew Howard
+ * Date: 10 Oct 2002
+ * CVS: $Id: plan.c 6591 2008-06-16 21:20:10Z gerkey $
+**************************************************************************/
+
+//#include <config.h>
+
+#ifndef MIN
+ #define MIN(a,b) ((a < b) ? (a) : (b))
+#endif
+#ifndef MAX
+ #define MAX(a,b) ((a > b) ? (a) : (b))
+#endif
+
+// This header MUST come before <openssl/md5.h>
+#include <sys/types.h>
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+ #include <openssl/md5.h>
+#endif
+
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+
+//#include <libplayercore/playercommon.h>
+//#include <libplayercore/error.h>
+
+#include "plan.h"
+//#include "heap.h"
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+// length of the hash, in unsigned ints
+#define HASH_LEN (MD5_DIGEST_LENGTH / sizeof(unsigned int))
+#endif
+
+#include <sys/time.h>
+static double get_time(void);
+
+#if 0
+void draw_cspace(plan_t* plan, const char* fname);
+#endif
+
+// Create a planner
+plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
+ double max_radius, double dist_penalty,
+ double hysteresis_factor)
+{
+ plan_t *plan;
+
+ plan = calloc(1, sizeof(plan_t));
+
+ plan->abs_min_radius = abs_min_radius;
+ plan->des_min_radius = des_min_radius;
+
+ plan->max_radius = max_radius;
+ plan->dist_penalty = dist_penalty;
+ plan->hysteresis_factor = hysteresis_factor;
+
+ plan->heap = heap_alloc(PLAN_DEFAULT_HEAP_SIZE, (heap_free_elt_fn_t)NULL);
+ assert(plan->heap);
+
+ plan->path_size = 1000;
+ plan->path = calloc(plan->path_size, sizeof(plan->path[0]));
+
+ plan->lpath_size = 100;
+ plan->lpath = calloc(plan->lpath_size, sizeof(plan->lpath[0]));
+
+ plan->waypoint_size = 100;
+ plan->waypoints = calloc(plan->waypoint_size, sizeof(plan->waypoints[0]));
+
+ return plan;
+}
+
+void
+plan_set_obstacles(plan_t* plan, double* obs, size_t num)
+{
+ size_t i;
+ int j;
+ int di,dj;
+ float* p;
+ plan_cell_t* cell, *ncell;
+ double t0,t1;
+
+ t0 = get_time();
+
+ // Start with static obstacle data
+ cell = plan->cells;
+ for(j=0;j<plan->size_y*plan->size_x;j++,cell++)
+ {
+ cell->occ_state_dyn = cell->occ_state;
+ cell->occ_dist_dyn = cell->occ_dist;
+ cell->mark = 0;
+ }
+
+ // Expand around the dynamic obstacle pts
+ for(i=0;i<num;i++)
+ {
+ // Convert to grid coords
+ int gx,gy;
+ gx = PLAN_GXWX(plan, obs[2*i]);
+ gy = PLAN_GYWY(plan, obs[2*i+1]);
+
+ if(!PLAN_VALID(plan,gx,gy))
+ continue;
+
+ cell = plan->cells + PLAN_INDEX(plan,gx,gy);
+
+ if(cell->mark)
+ continue;
+
+ cell->mark = 1;
+ cell->occ_state_dyn = 1;
+ cell->occ_dist_dyn = 0.0;
+
+ p = plan->dist_kernel;
+ for (dj = -plan->dist_kernel_width/2;
+ dj <= plan->dist_kernel_width/2;
+ dj++)
+ {
+ ncell = cell + -plan->dist_kernel_width/2 + dj*plan->size_x;
+ for (di = -plan->dist_kernel_width/2;
+ di <= plan->dist_kernel_width/2;
+ di++, p++, ncell++)
+ {
+ if(!PLAN_VALID_BOUNDS(plan,cell->ci+di,cell->cj+dj))
+ continue;
+
+ if(*p < ncell->occ_dist_dyn)
+ ncell->occ_dist_dyn = *p;
+ }
+ }
+ }
+
+ t1 = get_time();
+ //printf("plan_set_obstacles: %.6lf\n", t1-t0);
+}
+
+void
+plan_compute_dist_kernel(plan_t* plan)
+{
+ int i,j;
+ float* p;
+
+ // Compute variable sized kernel, for use in propagating distance from
+ // obstacles
+ plan->dist_kernel_width = 1 + 2 * (int)ceil(plan->max_radius / plan->scale);
+ plan->dist_kernel = (float*)realloc(plan->dist_kernel,
+ sizeof(float) *
+ plan->dist_kernel_width *
+ plan->dist_kernel_width);
+ assert(plan->dist_kernel);
+
+ p = plan->dist_kernel;
+ for(j=-plan->dist_kernel_width/2;j<=plan->dist_kernel_width/2;j++)
+ {
+ for(i=-plan->dist_kernel_width/2;i<=plan->dist_kernel_width/2;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ }
+ }
+ // also compute a 3x3 kernel, used when propagating distance from goal
+ p = plan->dist_kernel_3x3;
+ for(j=-1;j<=1;j++)
+ {
+ for(i=-1;i<=1;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ }
+ }
+}
+
+
+// Destroy a planner
+void plan_free(plan_t *plan)
+{
+ if (plan->cells)
+ free(plan->cells);
+ heap_free(plan->heap);
+ free(plan->waypoints);
+ if(plan->dist_kernel)
+ free(plan->dist_kernel);
+ free(plan);
+
+ return;
+}
+
+// Initialize the plan
+void plan_init(plan_t *plan)
+{
+ int i, j;
+ plan_cell_t *cell;
+
+ printf("scale: %.3lf\n", plan->scale);
+
+ cell = plan->cells;
+ for (j = 0; j < plan->size_y; j++)
+ {
+ for (i = 0; i < plan->size_x; i++, cell++)
+ {
+ cell->ci = i;
+ cell->cj = j;
+ cell->occ_state_dyn = cell->occ_state;
+ if(cell->occ_state >= 0)
+ cell->occ_dist_dyn = cell->occ_dist = 0.0;
+ else
+ cell->occ_dist_dyn = cell->occ_dist = plan->max_radius;
+ cell->plan_cost = PLAN_MAX_COST;
+ cell->plan_next = NULL;
+ cell->lpathmark = 0;
+ }
+ }
+ plan->waypoint_count = 0;
+
+ plan_compute_dist_kernel(plan);
+
+ plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
+}
+
+
+// Reset the plan
+void plan_reset(plan_t *plan)
+{
+ int i, j;
+ plan_cell_t *cell;
+
+ for (j = plan->min_y; j <= plan->max_y; j++)
+ {
+ for (i = plan->min_x; i <= plan->max_x; i++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan,i,j);
+ cell->plan_cost = PLAN_MAX_COST;
+ cell->plan_next = NULL;
+ cell->mark = 0;
+ }
+ }
+ plan->waypoint_count = 0;
+}
+
+void
+plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y)
+{
+ min_x = MAX(0,min_x);
+ min_x = MIN(plan->size_x-1, min_x);
+ min_y = MAX(0,min_y);
+ min_y = MIN(plan->size_y-1, min_y);
+ max_x = MAX(0,max_x);
+ max_x = MIN(plan->size_x-1, max_x);
+ max_y = MAX(0,max_y);
+ max_y = MIN(plan->size_y-1, max_y);
+
+ assert(min_x <= max_x);
+ assert(min_y <= max_y);
+
+ plan->min_x = min_x;
+ plan->min_y = min_y;
+ plan->max_x = max_x;
+ plan->max_y = max_y;
+
+ //printf("new bounds: (%d,%d) -> (%d,%d)\n",
+ //plan->min_x, plan->min_y,
+ //plan->max_x, plan->max_y);
+}
+
+int
+plan_check_inbounds(plan_t* plan, double x, double y)
+{
+ int gx, gy;
+
+ gx = PLAN_GXWX(plan, x);
+ gy = PLAN_GYWY(plan, y);
+
+ if((gx >= plan->min_x) && (gx <= plan->max_x) &&
+ (gy >= plan->min_y) && (gy <= plan->max_y))
+ return(1);
+ else
+ return(0);
+}
+
+void
+plan_set_bbox(plan_t* plan, double padding, double min_size,
+ double x0, double y0, double x1, double y1)
+{
+ int gx0, gy0, gx1, gy1;
+ int min_x, min_y, max_x, max_y;
+ int sx, sy;
+ int dx, dy;
+ int gmin_size;
+ int gpadding;
+
+ gx0 = PLAN_GXWX(plan, x0);
...
[truncated message content] |
|
From: <ge...@us...> - 2009-04-21 05:27:55
|
Revision: 14131
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14131&view=rev
Author: gerkey
Date: 2009-04-21 05:27:53 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
Fixed build
Modified Paths:
--------------
pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
Added Paths:
-----------
pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg
Added: pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg
===================================================================
--- pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg (rev 0)
+++ pkg/trunk/highlevel/robot_actions/msg/MoveBaseStateNew.msg 2009-04-21 05:27:53 UTC (rev 14131)
@@ -0,0 +1,10 @@
+Header header
+
+# Status
+ActionStatus status
+
+# Target position and orientation
+robot_msgs/PoseStamped goal
+
+# Actual position and orientation
+robot_msgs/PoseStamped feedback
Modified: pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h
===================================================================
--- pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-04-21 04:29:39 UTC (rev 14130)
+++ pkg/trunk/nav/people_aware_nav/include/people_aware_nav/move_base_constrained.h 2009-04-21 05:27:53 UTC (rev 14131)
@@ -39,6 +39,7 @@
#include <robot_actions/action_runner.h>
#include <people_aware_nav/ConstrainedMoveBaseState.h>
#include <robot_actions/Pose2D.h>
+#include <robot_msgs/PoseStamped.h>
#include <people_aware_nav/ConstrainedGoal.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
@@ -96,7 +97,7 @@
/**
* @brief Publish a path for visualization purposes
*/
- void publishPath(const std::vector<Pose2D>& path, std::string topic, double r, double g, double b, double a);
+ void publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
/**
* @brief Make a new global plan
@@ -133,7 +134,7 @@
costmap_2d::Costmap2D planner_cost_map_, controller_cost_map_;
navfn::NavfnROS* planner_;
- std::vector<Pose2D> global_plan_;
+ std::vector<robot_msgs::PoseStamped> global_plan_;
std::vector<robot_msgs::Point> footprint_;
std::string global_frame_, robot_base_frame_;
bool valid_plan_;
Modified: pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp
===================================================================
--- pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-04-21 04:29:39 UTC (rev 14130)
+++ pkg/trunk/nav/people_aware_nav/src/move_base_constrained.cpp 2009-04-21 05:27:53 UTC (rev 14131)
@@ -183,15 +183,26 @@
ROS_DEBUG_NAMED("move", "Modified costmap");
+
+ /*
std::vector<robot_actions::Pose2D> global_plan;
-
+
Pose2D goal_pose;
goal_pose.header = goal.header;
goal_pose.x = goal.x;
goal_pose.y = goal.y;
goal_pose.z = goal.z;
goal_pose.th = goal.th;
+ */
+
+ std::vector<robot_msgs::PoseStamped> global_plan;
+ robot_msgs::PoseStamped goal_pose;
+ goal_pose.header = goal.header;
+ tf::PoseTFToMsg(tf::Pose(btQuaternion(goal.th, 0, 0),
+ btVector3(goal.x, goal.y, goal.z)),
+ goal_pose.pose);
+
bool valid_plan = planner_->makePlan(goal_pose, global_plan);
ROS_DEBUG_NAMED("move", "Planner found plan");
@@ -233,15 +244,15 @@
void MoveBaseConstrained::prunePlan(){
lock_.lock();
- std::vector<robot_actions::Pose2D>::iterator it = global_plan_.begin();
+ std::vector<robot_msgs::PoseStamped>::iterator it = global_plan_.begin();
while(it != global_plan_.end()){
- const robot_actions::Pose2D& w = *it;
+ const robot_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- double x_diff = global_pose_.getOrigin().x() - w.x;
- double y_diff = global_pose_.getOrigin().y() - w.y;
+ double x_diff = global_pose_.getOrigin().x() - w.pose.position.x;
+ double y_diff = global_pose_.getOrigin().y() - w.pose.position.y;
double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
if(distance < 1){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.x, w.y);
+ ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.pose.position.x, w.pose.position.y);
break;
}
it = global_plan_.erase(it);
@@ -302,7 +313,7 @@
bool valid_control = false;
robot_msgs::PoseDot cmd_vel;
- std::vector<robot_actions::Pose2D> local_plan;
+ std::vector<robot_msgs::PoseStamped> local_plan;
//pass plan to controller
lock_.lock();
if(valid_plan_){
@@ -387,14 +398,14 @@
ros_node_.publish("robot_footprint", footprint_msg);
}
- void MoveBaseConstrained::publishPath(const std::vector<robot_actions::Pose2D>& path, std::string topic, double r, double g, double b, double a){
+ void MoveBaseConstrained::publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
// Extract the plan in world co-ordinates
robot_msgs::Polyline2D gui_path_msg;
gui_path_msg.header.frame_id = global_frame_;
gui_path_msg.set_points_size(path.size());
for(unsigned int i=0; i < path.size(); i++){
- gui_path_msg.points[i].x = path[i].x;
- gui_path_msg.points[i].y = path[i].y;
+ gui_path_msg.points[i].x = path[i].pose.position.x;
+ gui_path_msg.points[i].y = path[i].pose.position.y;
}
gui_path_msg.color.r = r;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-21 22:09:11
|
Revision: 14180
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14180&view=rev
Author: hsujohnhsu
Date: 2009-04-21 22:09:06 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
update per ticket 1190, transform timeout for nav stack.
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-21 22:09:06 UTC (rev 14180)
@@ -141,6 +141,7 @@
tf::Stamped<tf::Pose> global_pose_;
double inscribed_radius_, circumscribed_radius_, inflation_radius_;
double controller_frequency_;
+ double transform_tolerance_; // timeout before transform errors
};
};
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 22:09:06 UTC (rev 14180)
@@ -51,6 +51,7 @@
ros_node_.param("~global_frame", global_frame_, std::string("map"));
ros_node_.param("~robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~controller_frequency", controller_frequency_, 20.0);
+ ros_node_.param("~transform_tolerance", transform_tolerance_, 0.1);
//for display purposes
ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
@@ -182,7 +183,7 @@
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
+ robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
try{
tf_.transformPose(global_frame_, robot_pose, global_pose_);
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-21 22:09:06 UTC (rev 14180)
@@ -87,6 +87,7 @@
const costmap_2d::Costmap2D& cost_map_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
+ double transform_tolerance_; // timeout before transform errors
};
};
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 22:09:06 UTC (rev 14180)
@@ -42,6 +42,7 @@
//read parameters for the planner
ros_node_.param("~/navfn/global_frame", global_frame_, std::string("map"));
ros_node_.param("~/navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
+ ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.1);
}
double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
@@ -84,7 +85,7 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time::now();
+ robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
try{
//transform both the goal and pose of the robot to the global frame
tf_.transformPose(global_frame_, robot_pose, global_pose);
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d_ros.h 2009-04-21 22:09:06 UTC (rev 14180)
@@ -209,6 +209,7 @@
std::vector<ObservationBuffer*> clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
bool rolling_window_; ///< @brief Whether or not the costmap should roll with the robot
bool current_; ///< @brief Whether or not all the observation buffers are updating at the desired rate
+ double transform_tolerance_; // timeout before transform errors
};
};
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 22:00:23 UTC (rev 14179)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 22:09:06 UTC (rev 14180)
@@ -56,6 +56,8 @@
ros_node_.param("~" + prefix + "/costmap/global_frame", global_frame_, string("map"));
ros_node_.param("~" + prefix + "/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
+ ros_node_.param("~" + prefix + "/costmap/transform_tolerance", transform_tolerance_, 0.1);
+
//now we need to split the topics based on whitespace which we can use a stringstream for
stringstream ss(topics_string);
@@ -337,7 +339,7 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
+ robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
@@ -388,7 +390,7 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time();
+ robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-21 22:45:41
|
Revision: 14185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14185&view=rev
Author: hsujohnhsu
Date: 2009-04-21 22:45:36 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
planner transform timeout tolerance take 2. see ticket #1190.
Modified Paths:
--------------
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 22:39:34 UTC (rev 14184)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 22:45:36 UTC (rev 14185)
@@ -183,8 +183,8 @@
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
-
+ ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+ robot_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame_, robot_pose, global_pose_);
}
@@ -195,7 +195,8 @@
ROS_ERROR("Connectivity Error: %s\n", ex.what());
}
catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
}
}
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 22:39:34 UTC (rev 14184)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 22:45:36 UTC (rev 14185)
@@ -85,7 +85,8 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+ ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+ robot_pose.stamp_ = ros::Time();
try{
//transform both the goal and pose of the robot to the global frame
tf_.transformPose(global_frame_, robot_pose, global_pose);
@@ -100,8 +101,11 @@
return false;
}
catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- return false;
+ if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+ {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ return false;
+ }
}
double wx = global_pose.getOrigin().x();
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 22:39:34 UTC (rev 14184)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 22:45:36 UTC (rev 14185)
@@ -339,21 +339,20 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+ ros::Time current_time = ros::Time::now();
+ robot_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return;
}
catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- return;
+ if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
}
double wx = global_pose.getOrigin().x();
@@ -390,21 +389,20 @@
global_pose.setIdentity();
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- robot_pose.stamp_ = ros::Time::now() - ros::Duration().fromSec(transform_tolerance_);
+ ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
+ robot_pose.stamp_ = ros::Time();
try{
tf_.transformPose(global_frame_, robot_pose, global_pose);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return;
}
catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- return;
+ if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
}
double wx = global_pose.getOrigin().x();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-21 22:49:06
|
Revision: 14187
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14187&view=rev
Author: tfoote
Date: 2009-04-21 22:49:02 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
unused message and service
Removed Paths:
-------------
pkg/trunk/pr2_msgs/msg/OccDiff.msg
pkg/trunk/pr2_srvs/srv/TransientObstacles.srv
Deleted: pkg/trunk/pr2_msgs/msg/OccDiff.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/OccDiff.msg 2009-04-21 22:47:23 UTC (rev 14186)
+++ pkg/trunk/pr2_msgs/msg/OccDiff.msg 2009-04-21 22:49:02 UTC (rev 14187)
@@ -1,3 +0,0 @@
-Header header
-deprecated_msgs/Point2DFloat32[] occ_points
-deprecated_msgs/Point2DFloat32[] unocc_points
Deleted: pkg/trunk/pr2_srvs/srv/TransientObstacles.srv
===================================================================
--- pkg/trunk/pr2_srvs/srv/TransientObstacles.srv 2009-04-21 22:47:23 UTC (rev 14186)
+++ pkg/trunk/pr2_srvs/srv/TransientObstacles.srv 2009-04-21 22:49:02 UTC (rev 14187)
@@ -1,3 +0,0 @@
----
-time stamp
-robot_msgs/Polyline2D obs
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-04-21 23:10:30
|
Revision: 14191
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14191&view=rev
Author: hsujohnhsu
Date: 2009-04-21 23:10:26 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
adding return statements. move ROS_ERROR outside of if_timeout.
Modified Paths:
--------------
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 22:57:32 UTC (rev 14190)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-21 23:10:26 UTC (rev 14191)
@@ -190,13 +190,16 @@
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ return; // kind of pointless unless there's more code added below this try catch block
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ return;
}
catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ return;
}
}
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 22:57:32 UTC (rev 14190)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-21 23:10:26 UTC (rev 14191)
@@ -101,11 +101,9 @@
return false;
}
catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
return false;
- }
}
double wx = global_pose.getOrigin().x();
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 22:57:32 UTC (rev 14190)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-21 23:10:26 UTC (rev 14191)
@@ -346,13 +346,16 @@
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ return;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ return;
}
catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ return;
}
double wx = global_pose.getOrigin().x();
@@ -396,13 +399,16 @@
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
+ return;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
+ return;
}
catch(tf::ExtrapolationException& ex) {
+ ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
+ return;
}
double wx = global_pose.getOrigin().x();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-04-22 01:20:17
|
Revision: 14204
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14204&view=rev
Author: eitanme
Date: 2009-04-22 01:20:02 +0000 (Wed, 22 Apr 2009)
Log Message:
-----------
Moved a lot of code out of the action and into the ROS wrappers for the navstack
Modified Paths:
--------------
pkg/trunk/highlevel/nav/include/nav/move_base_action.h
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/highlevel/test_nav/launch_navstack.xml
pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_action.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_action.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -87,38 +87,19 @@
bool sleepLeftover(ros::Time start, ros::Duration cycle_time, ros::Duration& actual);
/**
- * @brief Publishes the footprint of the robot for visualization purposes
- */
- void publishFootprint();
-
- /**
- * @brief Publish a path for visualization purposes
- */
- void publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
-
- /**
* @brief Make a new global plan
* @param goal The goal to plan to
*/
void makePlan(const robot_msgs::PoseStamped& goal);
/**
- * @brief Trim off parts of the global plan that are far enough behind the robot
+ * @brief Get the current pose of the robot in the specified frame
+ * @param frame The frame to get the pose in
+ * @param pose The pose returned
*/
- void prunePlan();
+ void getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose);
/**
- * @brief Get the current pose of the robot in the global frame and set the global_pose_ variable
- */
- void updateGlobalPose();
-
- /**
- * @brief Clear the footprint of the robot in a given cost map
- * @param cost_map The costmap to apply the clearing opertaion on
- */
- void clearRobotFootprint(costmap_2d::Costmap2D& cost_map);
-
- /**
* @brief Resets the costmaps to the static map outside a given window
*/
void resetCostMaps();
@@ -133,15 +114,13 @@
navfn::NavfnROS* planner_;
std::vector<robot_msgs::PoseStamped> global_plan_;
std::vector<robot_msgs::Point> footprint_;
- std::string global_frame_, robot_base_frame_;
- bool valid_plan_;
+ std::string robot_base_frame_;
+ bool valid_plan_, new_plan_;
boost::recursive_mutex lock_;
robot_msgs::PoseStamped goal_;
tf::Stamped<tf::Pose> global_pose_;
- double inscribed_radius_, circumscribed_radius_, inflation_radius_;
double controller_frequency_;
- double transform_tolerance_; // timeout before transform errors
};
};
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -45,38 +45,20 @@
MoveBaseAction::MoveBaseAction(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
run_planner_(true), tc_(NULL), planner_cost_map_ros_(NULL), controller_cost_map_ros_(NULL),
- planner_(NULL), valid_plan_(false) {
+ planner_(NULL), valid_plan_(false), new_plan_(false) {
//get some parameters that will be global to the move base node
- ros_node_.param("~global_frame", global_frame_, std::string("map"));
- ros_node_.param("~robot_base_frame", robot_base_frame_, std::string("base_link"));
+ ros_node_.param("~navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~controller_frequency", controller_frequency_, 20.0);
- ros_node_.param("~transform_tolerance", transform_tolerance_, 0.1);
- //for display purposes
- ros_node_.advertise<robot_msgs::Polyline2D>("gui_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("local_path", 1);
- ros_node_.advertise<robot_msgs::Polyline2D>("robot_footprint", 1);
-
//for comanding the base
ros_node_.advertise<robot_msgs::PoseDot>("cmd_vel", 1);
- //pass on some parameters to the components of the move base node if they are not explicitly overridden
- //(perhaps the controller and the planner could operate in different frames)
- if(!ros_node_.hasParam("~base_local_planner/global_frame")) ros_node_.setParam("~base_local_planner/global_frame", global_frame_);
- if(!ros_node_.hasParam("~base_local_planner/robot_base_frame")) ros_node_.setParam("~base_local_planner/robot_base_frame", robot_base_frame_);
- if(!ros_node_.hasParam("~navfn/global_frame")) ros_node_.setParam("~navfn/global_frame", global_frame_);
- if(!ros_node_.hasParam("~navfn/robot_base_frame")) ros_node_.setParam("~navfn/robot_base_frame", robot_base_frame_);
+ double inscribed_radius, circumscribed_radius;
+ //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
+ ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius, 0.325);
+ ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius, 0.46);
- ros_node_.param("~inscribed_radius", inscribed_radius_, 0.325);
- ros_node_.param("~circumscribed_radius", circumscribed_radius_, 0.46);
- ros_node_.param("~inflation_radius", inflation_radius_, 0.55);
-
- //pass on inlfation parameters to the planner's costmap if they're not set explicitly
- if(!ros_node_.hasParam("~navfn/costmap/inscribed_radius")) ros_node_.setParam("~navfn/costmap/inscribed_radius", inscribed_radius_);
- if(!ros_node_.hasParam("~navfn/costmap/circumscribed_radius")) ros_node_.setParam("~navfn/costmap/circumscribed_radius", circumscribed_radius_);
- if(!ros_node_.hasParam("~navfn/costmap/inflation_radius")) ros_node_.setParam("~navfn/costmap/inflation_radius", inflation_radius_);
-
//create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
planner_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"));
planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
@@ -85,32 +67,27 @@
planner_ = new NavfnROS(ros_node_, tf_, planner_cost_map_);
ROS_INFO("MAP SIZE: %d, %d", planner_cost_map_.cellSizeX(), planner_cost_map_.cellSizeY());
- //pass on inlfation parameters to the controller's costmap if they're not set explicitly
- if(!ros_node_.hasParam("~base_local_planner/costmap/inscribed_radius")) ros_node_.setParam("~base_local_planner/costmap/inscribed_radius", inscribed_radius_);
- if(!ros_node_.hasParam("~base_local_planner/costmap/circumscribed_radius")) ros_node_.setParam("~base_local_planner/costmap/circumscribed_radius", circumscribed_radius_);
- if(!ros_node_.hasParam("~base_local_planner/costmap/inflation_radius")) ros_node_.setParam("~base_local_planner/costmap/inflation_radius", inflation_radius_);
-
//create the ros wrapper for the controller's cost_map... and initializer a pointer we'll use with the underlying map
controller_cost_map_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"));
controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
robot_msgs::Point pt;
//create a square footprint
- pt.x = inscribed_radius_ + .01;
- pt.y = -1 * (inscribed_radius_ + .01);
+ pt.x = inscribed_radius + .01;
+ pt.y = -1 * (inscribed_radius + .01);
footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + .01);
- pt.y = -1 * (inscribed_radius_ + .01);
+ pt.x = -1 * (inscribed_radius + .01);
+ pt.y = -1 * (inscribed_radius + .01);
footprint_.push_back(pt);
- pt.x = -1 * (inscribed_radius_ + .01);
- pt.y = inscribed_radius_ + .01;
+ pt.x = -1 * (inscribed_radius + .01);
+ pt.y = inscribed_radius + .01;
footprint_.push_back(pt);
- pt.x = inscribed_radius_ + .01;
- pt.y = inscribed_radius_ + .01;
+ pt.x = inscribed_radius + .01;
+ pt.y = inscribed_radius + .01;
footprint_.push_back(pt);
//give the robot a nose
- pt.x = circumscribed_radius_;
+ pt.x = circumscribed_radius;
pt.y = 0;
footprint_.push_back(pt);
@@ -134,35 +111,15 @@
delete controller_cost_map_ros_;
}
- void MoveBaseAction::clearRobotFootprint(Costmap2D& cost_map){
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- //get the oriented footprint of the robot
- std::vector<robot_msgs::Point> oriented_footprint = tc_->drawFootprint(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
-
- //set the associated costs in the cost map to be free
- if(!cost_map.setConvexPolygonCost(oriented_footprint, costmap_2d::FREE_SPACE))
- return;
-
- double max_inflation_dist = inflation_radius_ + inscribed_radius_;
-
- //make sure to re-inflate obstacles in the affected region
- cost_map.reinflateWindow(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), max_inflation_dist, max_inflation_dist);
-
- }
-
void MoveBaseAction::makePlan(const robot_msgs::PoseStamped& goal){
//since this gets called on handle activate
if(planner_cost_map_ros_ == NULL)
return;
- //make a plan for controller
+ //update the copy of the costmap the planner uses
planner_cost_map_ros_->getCostMapCopy(planner_cost_map_);
- //make sure we clear the robot's footprint from the cost map
- clearRobotFootprint(planner_cost_map_);
-
std::vector<robot_msgs::PoseStamped> global_plan;
bool valid_plan = planner_->makePlan(goal, global_plan);
@@ -173,20 +130,19 @@
lock_.lock();
//copy over the new global plan
valid_plan_ = valid_plan;
+ new_plan_ = true;
global_plan_ = global_plan;
lock_.unlock();
-
- publishPath(global_plan, "gui_path", 0.0, 1.0, 0.0, 0.0);
}
- void MoveBaseAction::updateGlobalPose(){
+ void MoveBaseAction::getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose){
tf::Stamped<tf::Pose> robot_pose;
robot_pose.setIdentity();
robot_pose.frame_id_ = robot_base_frame_;
- ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
robot_pose.stamp_ = ros::Time();
+
try{
- tf_.transformPose(global_frame_, robot_pose, global_pose_);
+ tf_.transformPose(frame, robot_pose, pose);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
@@ -198,83 +154,57 @@
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- return;
}
}
- void MoveBaseAction::prunePlan(){
- lock_.lock();
- std::vector<robot_msgs::PoseStamped>::iterator it = global_plan_.begin();
- while(it != global_plan_.end()){
- const robot_msgs::PoseStamped& w = *it;
- // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- double x_diff = global_pose_.getOrigin().x() - w.pose.position.x;
- double y_diff = global_pose_.getOrigin().y() - w.pose.position.y;
- double distance = sqrt(x_diff * x_diff + y_diff * y_diff);
- if(distance < 1){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), w.pose.position.x, w.pose.position.y);
- break;
- }
- it = global_plan_.erase(it);
- }
- lock_.unlock();
- }
-
robot_actions::ResultStatus MoveBaseAction::execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback){
//update the goal
goal_ = goal;
- //update the global pose
- updateGlobalPose();
-
//first... make a plan to the goal
makePlan(goal_);
ros::Duration cycle_time = ros::Duration(1.0 / controller_frequency_);
while(!isPreemptRequested()){
+ struct timeval start, end;
+ double start_t, end_t, t_diff;
+ gettimeofday(&start, NULL);
+
//get the start time of the loop
ros::Time start_time = ros::Time::now();
- //update the global pose
- updateGlobalPose();
-
//update feedback to correspond to our current position
- tf::PoseStampedTFToMsg(global_pose_, feedback);
+ tf::Stamped<tf::Pose> global_pose;
+ getRobotPose(goal_.header.frame_id, global_pose);
+ tf::PoseStampedTFToMsg(global_pose, feedback);
//push the feedback out
update(feedback);
-
//make sure to update the cost_map we'll use for this cycle
controller_cost_map_ros_->getCostMapCopy(controller_cost_map_);
- //make sure that we clear the robot footprint in the cost map
- clearRobotFootprint(controller_cost_map_);
-
- //prune the plan before we pass it to the controller
- prunePlan();
-
- struct timeval start, end;
- double start_t, end_t, t_diff;
- gettimeofday(&start, NULL);
-
//check that the observation buffers for the costmap are current
if(!controller_cost_map_ros_->isCurrent()){
ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
continue;
}
+
bool valid_control = false;
robot_msgs::PoseDot cmd_vel;
- std::vector<robot_msgs::PoseStamped> local_plan;
//pass plan to controller
lock_.lock();
if(valid_plan_){
+ //if we have a new plan... we'll update the plan for the controller
+ if(new_plan_){
+ tc_->updatePlan(global_plan_);
+ new_plan_ = false;
+ }
//get observations for the non-costmap controllers
std::vector<Observation> observations;
controller_cost_map_ros_->getMarkingObservations(observations);
- valid_control = tc_->computeVelocityCommands(global_plan_, cmd_vel, local_plan, observations);
+ valid_control = tc_->computeVelocityCommands(cmd_vel, observations);
}
else{
//we don't have a valid plan... so we want to stop
@@ -298,11 +228,6 @@
makePlan(goal_);
}
- //for visualization purposes
- publishPath(global_plan_, "gui_path", 0.0, 1.0, 0.0, 0.0);
- publishPath(local_plan, "local_path", 0.0, 0.0, 1.0, 0.0);
- publishFootprint();
-
gettimeofday(&end, NULL);
start_t = start.tv_sec + double(start.tv_usec) / 1e6;
end_t = end.tv_sec + double(end.tv_usec) / 1e6;
@@ -339,47 +264,6 @@
controller_cost_map_ros_->resetMapOutsideWindow(5.0, 5.0);
}
- void MoveBaseAction::publishFootprint(){
- double useless_pitch, useless_roll, yaw;
- global_pose_.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
- std::vector<robot_msgs::Point> footprint = tc_->drawFootprint(global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw);
- robot_msgs::Polyline2D footprint_msg;
- footprint_msg.header.frame_id = global_frame_;
- footprint_msg.set_points_size(footprint.size());
- footprint_msg.color.r = 1.0;
- footprint_msg.color.g = 0;
- footprint_msg.color.b = 0;
- footprint_msg.color.a = 0;
- for(unsigned int i = 0; i < footprint.size(); ++i){
- footprint_msg.points[i].x = footprint[i].x;
- footprint_msg.points[i].y = footprint[i].y;
- }
- ros_node_.publish("robot_footprint", footprint_msg);
- }
-
- void MoveBaseAction::publishPath(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a){
- //given an empty path we won't do anything
- if(path.empty())
- return;
-
- // Extract the plan in world co-ordinates, we assume the path is all in the same frame
- robot_msgs::Polyline2D gui_path_msg;
- gui_path_msg.header.frame_id = path[0].header.frame_id;
- gui_path_msg.header.stamp = path[0].header.stamp;
- gui_path_msg.set_points_size(path.size());
- for(unsigned int i=0; i < path.size(); i++){
- gui_path_msg.points[i].x = path[i].pose.position.x;
- gui_path_msg.points[i].y = path[i].pose.position.y;
- }
-
- gui_path_msg.color.r = r;
- gui_path_msg.color.g = g;
- gui_path_msg.color.b = b;
- gui_path_msg.color.a = a;
-
- ros_node_.publish(topic, gui_path_msg);
- }
-
};
int main(int argc, char** argv){
Modified: pkg/trunk/highlevel/test_nav/launch_navstack.xml
===================================================================
--- pkg/trunk/highlevel/test_nav/launch_navstack.xml 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/highlevel/test_nav/launch_navstack.xml 2009-04-22 01:20:02 UTC (rev 14204)
@@ -3,6 +3,10 @@
<group name="wg">
<node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
<remap from="/move_base_node/activate" to="/goal" />
+ <remap from="~base_local_planner/global_plan" to="/gui_path" />
+ <remap from="~base_local_planner/local_plan" to="/local_path" />
+ <remap from="~base_local_planner/robot_footprint" to="/robot_footprint" />
+
<param name="global_frame" value="map" />
<param name="robot_base_frame" value="base_link" />
Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -42,6 +42,7 @@
#include <costmap_2d/costmap_2d.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/Point.h>
+#include <robot_msgs/Polyline2D.h>
#include <tf/transform_listener.h>
#include <vector>
#include <robot_msgs/Point.h>
@@ -55,7 +56,7 @@
* @param tf A reference to a TransformListener
* @param cos_map A reference to the costmap to use
*/
- NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, const costmap_2d::Costmap2D& cost_map);
+ NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map);
/**
* @brief Given a goal pose in the world, compute a plan
@@ -79,15 +80,22 @@
*/
double getPointPotential(const robot_msgs::Point& world_point);
+ /**
+ * @brief Publish a path for visualization purposes
+ */
+ void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, double r, double g, double b, double a);
+
~NavfnROS(){}
private:
+ void clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my);
ros::Node& ros_node_;
tf::TransformListener& tf_;
- const costmap_2d::Costmap2D& cost_map_;
+ costmap_2d::Costmap2D& cost_map_;
NavFn planner_;
std::string global_frame_, robot_base_frame_;
double transform_tolerance_; // timeout before transform errors
+ double inscribed_radius_, circumscribed_radius_, inflation_radius_;
};
};
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -37,12 +37,20 @@
#include <navfn/navfn_ros.h>
namespace navfn {
- NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, const costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
+ NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2D& cost_map) : ros_node_(ros_node), tf_(tf),
cost_map_(cost_map), planner_(cost_map.cellSizeX(), cost_map.cellSizeY()) {
+ //advertise our plan visualization
+ ros_node_.advertise<robot_msgs::Polyline2D>("~navfn/plan", 1);
+
//read parameters for the planner
ros_node_.param("~/navfn/global_frame", global_frame_, std::string("map"));
ros_node_.param("~/navfn/robot_base_frame", robot_base_frame_, std::string("base_link"));
ros_node_.param("~/navfn/transform_tolerance", transform_tolerance_, 0.1);
+
+ //we'll get the parameters for the robot radius from the costmap we're associated with
+ ros_node_.param("~navfn/costmap/inscribed_radius", inscribed_radius_, 0.325);
+ ros_node_.param("~navfn/costmap/circumscribed_radius", circumscribed_radius_, 0.46);
+ ros_node_.param("~navfn/costmap/inflation_radius", circumscribed_radius_, 0.55);
}
double NavfnROS::getPointPotential(const robot_msgs::Point& world_point){
@@ -75,6 +83,20 @@
return planner_.calcNavFnDijkstra();
}
+ void NavfnROS::clearRobotCell(const tf::Stamped<tf::Pose>& global_pose, unsigned int mx, unsigned int my){
+ double useless_pitch, useless_roll, yaw;
+ global_pose.getBasis().getEulerZYX(yaw, useless_pitch, useless_roll);
+
+ //set the associated costs in the cost map to be free
+ cost_map_.setCost(mx, my, costmap_2d::FREE_SPACE);
+
+ double max_inflation_dist = inflation_radius_ + inscribed_radius_;
+
+ //make sure to re-inflate obstacles in the affected region
+ cost_map_.reinflateWindow(global_pose.getOrigin().x(), global_pose.getOrigin().y(), max_inflation_dist, max_inflation_dist);
+
+ }
+
bool NavfnROS::makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan){
//get the pose of the robot in the global frame
tf::Stamped<tf::Pose> robot_pose, global_pose, orig_goal, goal_pose;
@@ -109,12 +131,15 @@
double wx = global_pose.getOrigin().x();
double wy = global_pose.getOrigin().y();
- planner_.setCostMap(cost_map_.getCharMap());
-
unsigned int mx, my;
if(!cost_map_.worldToMap(wx, wy, mx, my))
return false;
+ //clear the starting cell within the costmap because we know it can't be an obstacle
+ clearRobotCell(global_pose, mx, my);
+
+ planner_.setCostMap(cost_map_.getCharMap());
+
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
@@ -157,6 +182,31 @@
}
}
+ //publish the plan for visualization purposes
+ publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
return success;
}
+
+ void NavfnROS::publishPlan(const std::vector<robot_msgs::PoseStamped>& path, double r, double g, double b, double a){
+ //given an empty path we won't do anything
+ if(path.empty())
+ return;
+
+ // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+ robot_msgs::Polyline2D gui_path_msg;
+ gui_path_msg.header.frame_id = path[0].header.frame_id;
+ gui_path_msg.header.stamp = path[0].header.stamp;
+ gui_path_msg.set_points_size(path.size());
+ for(unsigned int i=0; i < path.size(); i++){
+ gui_path_msg.points[i].x = path[i].pose.position.x;
+ gui_path_msg.points[i].y = path[i].pose.position.y;
+ }
+
+ gui_path_msg.color.r = r;
+ gui_path_msg.color.g = g;
+ gui_path_msg.color.b = b;
+ gui_path_msg.color.a = a;
+
+ ros_node_.publish("~navfn/plan", gui_path_msg);
+ }
};
Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h 2009-04-22 01:20:02 UTC (rev 14204)
@@ -53,6 +53,7 @@
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/Point.h>
+#include <robot_msgs/Polyline2D.h>
#include <laser_scan/LaserScan.h>
#include <tf/message_notifier.h>
@@ -81,7 +82,7 @@
* @param planner_map Used to size the map for the freespace controller... this will go away once a rolling window version of the point grid is in place
*/
TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- const costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
+ costmap_2d::Costmap2D& cost_map, std::vector<robot_msgs::Point> footprint_spec,
const costmap_2d::Costmap2D* planner_map = NULL);
/**
@@ -100,18 +101,20 @@
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
- * @param orig_global_plan The plan to pass to the controller
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
- * @param local_plan Will be set from the points of the selected trajectory for display purposes
* @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
* @return True if a valid trajectory was found, false otherwise
*/
- bool computeVelocityCommands(const std::vector<robot_msgs::PoseStamped>& orig_global_plan,
- robot_msgs::PoseDot& cmd_vel,
- std::vector<robot_msgs::PoseStamped>& local_plan,
+ bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel,
const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0));
/**
+ * @brief Update the plan that the controller is following
+ * @param orig_global_plan The plan to pass to the controller
+ */
+ void updatePlan(const std::vector<robot_msgs::PoseStamped>& orig_global_plan);
+
+ /**
* @brief Returns the local goal the robot is pursuing
* @param x Will be set to the x position of the goal in world coordinates
* @param y Will be set to the y position of the goal in world coordinates
@@ -163,20 +166,45 @@
* @param y1 The first y point
* @param x2 The second x point
* @param y2 The second y point
- * @return
*/
double distance(double x1, double y1, double x2, double y2);
+ /**
+ * @brief Trim off parts of the global plan that are far enough behind the robot
+ * @param global_pose The pose of the robot in the global frame
+ * @param plan The plan to be pruned
+ */
+ void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<robot_msgs::PoseStamped>& plan);
+
+ /**
+ * @brief Publishes the footprint of the robot for visualization purposes
+ * @param global_pose The pose of the robot in the global frame
+ */
+ void publishFootprint(const tf::Stamped<tf::Pose>& global_pose);
+
+ /**
+ * @brief Clear the footprint of the robot in a given cost map
+ * @param global_pose The pose of the robot in the global frame
+ * @param cost_map The costmap to apply the clearing opertaion on
+ */
+ void clearRobotFootprint(const tf::Stamped<tf::Pose>& global_pose, costmap_2d::Costmap2D& cost_map);
+
+ /**
+ * @brief Publish a plan for visualization purposes
+ */
+ void publishPlan(const std::vector<robot_msgs::PoseStamped>& path, std::string topic, double r, double g, double b, double a);
+
void baseScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void tiltScanCallback(const tf::MessageNotifier<laser_scan::LaserScan>::MessagePtr& message);
void odomCallback();
-
WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
+ costmap_2d::Costmap2D& cost_map_; ///< @brief The costmap the controller will use
tf::MessageNotifier<laser_scan::LaserScan>* base_scan_notifier_; ///< @brief Used to guarantee that a transform is available for base scans
tf::MessageNotifier<laser_scan::LaserScan>* tilt_scan_notifier_; ///< @brief Used to guarantee that a transform is available for tilt scans
tf::TransformListener& tf_; ///< @brief Used for transforming point clouds
+ ros::Node& ros_node_; ///< @brief The ros node we're running under
std::string global_frame_; ///< @brief The frame in which the controller will run
laser_scan::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds
boost::recursive_mutex obs_lock_; ///< @brief Lock for accessing data in callbacks safely
@@ -188,7 +216,9 @@
std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
double rot_stopped_velocity_, trans_stopped_velocity_;
double xy_goal_tolerance_, yaw_goal_tolerance_, min_in_place_vel_th_;
+ double inscribed_radius_, circumscribed_radius_, inflation_radius_;
bool goal_reached_;
+ std::vector<robot_msgs::PoseStamped> global_plan_;
};
};
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-22 01:04:19 UTC (rev 14203)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp 2009-04-22 01:20:02 UTC (rev 14204)
@@ -46,8 +46,8 @@
namespace base_local_planner {
TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf,
- const Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
- : world_model_(NULL), tc_(NULL), base_scan_notifier_(NULL), tf_(tf), laser_scans_(2),
+ Costmap2D& cost_map, std::vector<Point> footprint_spec, const Costmap2D* planner_map)
+ : world_model_(NULL), tc_(NULL), cost_map_(cost_map), base_s...
[truncated message content] |
|
From: <hsu...@us...> - 2009-04-22 19:26:16
|
Revision: 14237
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14237&view=rev
Author: hsujohnhsu
Date: 2009-04-22 19:26:06 +0000 (Wed, 22 Apr 2009)
Log Message:
-----------
fix on timeout catch for TF transforms. appears to be working in sim now.
Modified Paths:
--------------
pkg/trunk/highlevel/nav/src/move_base_action.cpp
pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
Modified: pkg/trunk/highlevel/nav/src/move_base_action.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/highlevel/nav/src/move_base_action.cpp 2009-04-22 19:26:06 UTC (rev 14237)
@@ -146,7 +146,7 @@
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return; // kind of pointless unless there's more code added below this try catch block
+ return;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp 2009-04-22 19:26:06 UTC (rev 14237)
@@ -124,9 +124,18 @@
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- return false;
+ return false;
}
+ // check global pose timeout
+ if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+ ROS_ERROR("Transform timeout. global psoe stamp: %f current time: %f",global_pose.stamp_.toSec(),current_time.toSec());
+ return false;
+ }
+ // check goal pose timeout
+ if (current_time.toSec() - goal_pose.stamp_.toSec() > transform_tolerance_) {
+ ROS_ERROR("Transform timeout. goal psoe stamp: %f current time: %f",goal_pose.stamp_.toSec(),current_time.toSec());
+ return false;
+ }
double wx = global_pose.getOrigin().x();
double wy = global_pose.getOrigin().y();
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-22 19:17:07 UTC (rev 14236)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2009-04-22 19:26:06 UTC (rev 14237)
@@ -354,9 +354,16 @@
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- return;
+ return;
}
+ // check global_pose timeout
+ if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+ ROS_ERROR("Transform timeout.");
+ ROS_ERROR(" current time : %f",current_time.toSec());
+ ROS_ERROR(" globalpose stamp: %f",global_pose.stamp_.toSec());
+ ROS_ERROR(" tolerance : %f",transform_tolerance_);
+ return;
+ }
double wx = global_pose.getOrigin().x();
double wy = global_pose.getOrigin().y();
@@ -407,9 +414,16 @@
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (current_time - robot_pose.stamp_ > ros::Duration().fromSec(transform_tolerance_))
- return;
+ return;
}
+ // check global_pose timeout
+ if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_) {
+ ROS_ERROR("Transform timeout.");
+ ROS_ERROR(" current time : %f",current_time.toSec());
+ ROS_ERROR(" globalpose stamp: %f",global_pose.stamp_.toSec());
+ ROS_ERROR(" tolerance : %f",transform_tolerance_);
+ return;
+ }
double wx = global_pose.getOrigin().x();
double wy = global_pose.getOrigin().y();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-04-22 20:52:21
|
Revision: 14258
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14258&view=rev
Author: tfoote
Date: 2009-04-22 20:52:11 +0000 (Wed, 22 Apr 2009)
Log Message:
-----------
moving amcl_player to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/amcl_player/
Removed Paths:
-------------
pkg/trunk/nav/amcl_player/
Property changes on: pkg/trunk/deprecated/amcl_player
___________________________________________________________________
Added: svn:ignore
+ cli
amcl_player
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-04-23 00:05:30
|
Revision: 14291
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14291&view=rev
Author: gerkey
Date: 2009-04-23 00:05:27 +0000 (Thu, 23 Apr 2009)
Log Message:
-----------
Pushed params into local namespace for fake_localization and wavefront;
updated launch scripts accordingly. Removed amcl_player from 2dnav_stage.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/nav/amcl/src/amcl_node.cpp
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Added Paths:
-----------
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch
Removed Paths:
-------------
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -3,11 +3,11 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront" type="wavefront" respawn="false" >
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <param name="max_publish_frequency" value="20.0"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -5,9 +5,9 @@
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
<node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
- <param name="max_publish_frequency" value="20.0"/>
<node pkg="fake_localization" type="fake_localization" respawn="false" />
</group>
</launch>
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-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -15,7 +15,6 @@
</node>
<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"/>
<param name="robot_0/fake_localization_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
<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" />
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -3,21 +3,40 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="20"/>
- <param name="pf_min_samples" value="100"/>
- <param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.5"/>
- <param name="pf_odom_drift_yy" value="0.5"/>
- <param name="pf_odom_drift_aa" value="0.5"/>
- <param name="pf_odom_drift_xa" value="0.5"/>
- <param name="pf_min_d" value="0.25"/> <!-- 25cm -->
- <param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl" respawn="false" output="screen">
+ <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
+ <param name="laser_max_beams" value="30"/>
+ <param name="min_particles" value="100"/>
+ <param name="max_particles" value="5000"/>
+ <param name="kld_err" value="0.05"/>
+ <param name="kld_z" value="0.99"/>
+ <param name="odom_alpha1" value="0.2"/>
+ <param name="odom_alpha2" value="0.2"/>
+ <param name="odom_alpha3" value="0.2"/>
+ <param name="odom_alpha4" value="0.2"/>
+ <param name="laser_z_hit" value="0.95"/>
+ <param name="laser_z_short" value="0.1"/>
+ <param name="laser_z_max" value="0.05"/>
+ <param name="laser_z_rand" value="0.05"/>
+ <param name="laser_sigma_hit" value="0.3"/>
+ <param name="laser_lambda_short" value="0.1"/>
+ <param name="laser_lambda_short" value="0.1"/>
+ <param name="laser_model_type" value="likelihood_field"/>
+ <param name="laser_likelihood_max_dist" value="2.0"/>
+ <param name="update_min_d" value="0.2"/>
+ <param name="update_min_a" value="0.5"/>
+ <param name="odom_frame_id" value="odom"/>
+ <param name="resample_interval" value="2"/>
+ <param name="transform_tolerance" value="0.1"/>
+ <param name="recovery_alpha_slow" value="0.0"/>
+ <param name="recovery_alpha_fast" value="0.0"/>
+ <param name="robot_x_start" value="0.0"/>
+ <param name="robot_y_start" value="0.0"/>
+ <param name="robot_th_start" value="0.0"/>
</node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
<!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/--><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
Deleted: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,26 +0,0 @@
-<launch>
-
- <group name="wg">
- <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-2.5cm.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.025.pgm 0.025" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="6"/>
- <param name="pf_min_samples" value="100"/>
- <param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.1"/>
- <param name="pf_odom_drift_yy" value="0.1"/>
- <param name="pf_odom_drift_aa" value="0.1"/>
- <param name="pf_odom_drift_xa" value="0.1"/>
- <param name="pf_min_d" value="0.25"/> <!-- 25cm -->
- <param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> --><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
- </group>
-</launch>
-
Copied: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch (from rev 14280, pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch)
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch (rev 0)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -0,0 +1,17 @@
+<launch>
+
+ <group name="wg">
+ <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" output="screen"/>
+ <node pkg="amcl" type="amcl" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <node pkg="wavefront" type="wavefront" name="wavefront" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ <param name="robot_radius" value="0.325"/>
+ </node>
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> --><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
+ </group>
+</launch>
+
Property changes on: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_5cm.launch
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,45 +0,0 @@
-<launch>
-
- <group name="wg">
- <node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- <param name="laser_max_beams" value="30"/>
- <param name="min_particles" value="100"/>
- <param name="max_particles" value="5000"/>
- <param name="kld_err" value="0.05"/>
- <param name="kld_z" value="0.99"/>
- <param name="odom_alpha1" value="0.2"/>
- <param name="odom_alpha2" value="0.2"/>
- <param name="odom_alpha3" value="0.2"/>
- <param name="odom_alpha4" value="0.2"/>
- <param name="laser_z_hit" value="0.95"/>
- <param name="laser_z_short" value="0.1"/>
- <param name="laser_z_max" value="0.05"/>
- <param name="laser_z_rand" value="0.05"/>
- <param name="laser_sigma_hit" value="0.3"/>
- <param name="laser_lambda_short" value="0.1"/>
- <param name="laser_lambda_short" value="0.1"/>
- <param name="laser_model_type" value="likelihood_field"/>
- <param name="laser_likelihood_max_dist" value="2.0"/>
- <param name="update_min_d" value="0.2"/>
- <param name="update_min_a" value="0.5"/>
- <param name="odom_frame_id" value="odom"/>
- <param name="resample_interval" value="10"/>
- <param name="transform_tolerance" value="0.1"/>
- <param name="recovery_alpha_slow" value="0.001"/>
- <param name="recovery_alpha_fast" value="0.1"/>
- <param name="robot_x_start" value="0.0"/>
- <param name="robot_y_start" value="0.0"/>
- <param name="robot_th_start" value="0.0"/>
- </node>
- <param name="robot_radius" value="0.325"/>
- <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
- <remap from="scan" to="base_scan" />
- </node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/--><!-- You must have nav_view_sdl installed it is not a tracked dependency-->
- </group>
-</launch>
-
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-23 00:05:27 UTC (rev 14291)
@@ -6,8 +6,6 @@
<url>http://pr.willowgarage.com/wiki/FIXME</url>
<depend package="nav_view"/>
<depend package="amcl"/>
- <!-- TODO: remove amcl_player -->
- <depend package="amcl_player"/>
<depend package="fake_localization"/>
<depend package="stage"/>
<depend package="map_server"/>
Modified: pkg/trunk/nav/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/amcl/src/amcl_node.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -84,10 +84,10 @@
- @b "~kld_z" (double) : Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err, default: 0.99
- @b "~update_min_d" (double) : Translational movement required before performing a filter update, default: 0.2 meters
- @b "~update_min_a" (double) : Rotational movement required before performing a filter update, default: M_PI/6.0 radians
- - @b "~resample_interval" (int) : Number of filter updates required before resampling, default: 10
+ - @b "~resample_interval" (int) : Number of filter updates required before resampling, default: 2
- @b "~transform_tolerance" (double) : Time with which to post-date the transform that is published, to indicate that this transform is valid into the future, default: 0.1 seconds
- - @b "~recovery_alpha_slow" (double) : Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses, default: 0.001
- - @b "~recovery_alpha_fast" (double) : Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses, default: 0.1
+ - @b "~recovery_alpha_slow" (double) : Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses, default: 0.0, which means disabled (a good value might be 0.001)
+ - @b "~recovery_alpha_fast" (double) : Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses, default: 0.0, which means disabled (a good value might be 0.1)
- @b "~initial_pose_x" (double) : Initial pose estimate (x), used to initialize filter with Gaussian distribution, default: 0.0 meters
- @b "~initial_pose_y" (double) : Initial pose estimate (y), used to initialize filter with Gaussian distribution, default: 0.0 meters
- @b "~initial_pose_a" (double) : Initial pose estimate (yaw), used to initialize filter with Gaussian distribution, default: 0.0 radians
@@ -302,7 +302,7 @@
ros::Node::instance()->param("~update_min_d", d_thresh_, 0.2);
ros::Node::instance()->param("~update_min_a", a_thresh_, M_PI/6.0);
ros::Node::instance()->param("~odom_frame_id", odom_frame_id_, std::string("odom"));
- ros::Node::instance()->param("~resample_interval", resample_interval_, 10);
+ ros::Node::instance()->param("~resample_interval", resample_interval_, 2);
double tmp_tol;
ros::Node::instance()->param("~transform_tolerance", tmp_tol, 0.1);
ros::Node::instance()->param("~recovery_alpha_slow", alpha_slow, 0.001);
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -40,13 +40,15 @@
@htmlinclude manifest.html
-@b odom_localization is simply forwards the odometry information
+@b odom_localization Takes in ground truth pose information for a robot
+base (e.g., from a simulator or motion capture system) and republishes
+it as if a localization system were in use.
<hr>
@section usage Usage
@verbatim
-$ odom_localization
+$ fake_localization
@endverbatim
<hr>
@@ -54,28 +56,26 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth"/robot_msgs::PoseWithRatesStamped : 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).
+- @b "base_pose_ground_truth" robot_msgs/PoseWithRatesStamped : 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).
-- @b "particlecloud"/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
+- @b "amcl_pose" robot_msgs/PoseWithCovariance : robot's estimated pose in the map, with covariance
+- @b "particlecloud" robot_msgs/ParticleCloud : fake set of particles being maintained by the filter (one paricle only).
<hr>
@section parameters ROS parameters
-- None
+- "~odom_frame_id" (string) : The odometry frame to be used, default: "odom"
**/
#include <ros/node.h>
#include <ros/time.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
#include <robot_msgs/PoseWithRatesStamped.h>
#include <robot_msgs/ParticleCloud.h>
-#include <deprecated_msgs/Pose2DFloat32.h>
+#include <robot_msgs/PoseWithCovariance.h>
#include <angles/angles.h>
@@ -91,7 +91,7 @@
public:
FakeOdomNode(void) : ros::Node("fake_localization")
{
- advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose",1);
+ advertise<robot_msgs::PoseWithCovariance>("amcl_pose",1);
advertise<robot_msgs::ParticleCloud>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster(*this);
m_tfListener = new tf::TransformListener(*this);
@@ -99,8 +99,7 @@
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;
+ param("~odom_frame_id", odom_frame_id_, std::string("odom"));
m_particleCloud.set_particles_size(1);
notifier = new tf::MessageNotifier<robot_msgs::PoseWithRatesStamped>(m_tfListener, this,
boost::bind(&FakeOdomNode::update, this, _1),
@@ -139,8 +138,7 @@
robot_msgs::PoseWithRatesStamped m_basePosMsg;
robot_msgs::ParticleCloud m_particleCloud;
- deprecated_msgs::RobotBase2DOdom m_currentPos;
- deprecated_msgs::Pose2DFloat32 m_iniPos;
+ robot_msgs::PoseWithCovariance m_currentPos;
//parameter for what odom to use
std::string odom_frame_id_;
@@ -163,13 +161,13 @@
message->pos.position.y,
0.0*message->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 x = txi.getOrigin().x();
+ double y = txi.getOrigin().y();
double z = txi.getOrigin().z();
double yaw, pitch, roll;
txi.getBasis().getEulerZYX(yaw, pitch, roll);
- yaw = angles::normalize_angle(yaw + m_iniPos.th);
+ yaw = angles::normalize_angle(yaw);
tf::Transform txo(tf::Quaternion(yaw, pitch, roll), tf::Point(x, y, z));
@@ -204,16 +202,16 @@
// Publish localized pose
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;
- publish("localizedpose", m_currentPos);
+ m_currentPos.pose.position.x = x;
+ m_currentPos.pose.position.y = y;
+ // Leave z as zero
+ tf::QuaternionTFToMsg(tf::Quaternion(yaw, 0.0, 0.0),
+ m_currentPos.pose.orientation);
+ // Leave covariance as zero
+ publish("amcl_pose", m_currentPos);
// The particle cloud is the current position. Quite convenient.
- robot_msgs::Pose pos;
- tf::PoseTFToMsg(tf::Pose(tf::Quaternion(m_currentPos.pos.th, 0, 0), tf::Vector3(m_currentPos.pos.x, m_currentPos.pos.y, 0)),
- pos);
- m_particleCloud.particles[0] = pos;
+ m_particleCloud.particles[0] = m_currentPos.pose;
publish("particlecloud", m_particleCloud);
}
};
Modified: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,5 +1,5 @@
<package>
- <description>A ROS node that uses the Player libwavefront_standalone library, which implements wavefront path-planning for a planar robot.</description>
+ <description>A ROS node that uses the Player wavefront library, which implements wavefront path-planning for a planar robot.</description>
<author>Brian P. Gerkey</author>
<license>LGPL</license>
<review status="unreviewed" notes=""/>
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-22 23:50:21 UTC (rev 14290)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-23 00:05:27 UTC (rev 14291)
@@ -1,31 +1,21 @@
/*
- * wavefront_player
+ * wavefront
* 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:
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
*
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the <ORGANIZATION> nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
*
- * 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.
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/**
@@ -34,7 +24,7 @@
@htmlinclude manifest.html
-@b wavefront_player is a path-planning system for a robot moving in 2D. It
+@b wavefront is a path-planning system for a robot moving in 2D. It
implements the wavefront algorithm (described in various places; Latombe's
book is a good reference), which uses dynamic programming over an occupancy
grid map to find the lowest-cost path from the robot's pose to the goal.
@@ -43,8 +33,7 @@
documentation, consult <a
href="http://playerstage.sourceforge.net/doc/Player-cvs/player/group__driver__wavefront.html">Player
wavefront documentation</a>. Note that this node does not actually wrap the @b
-wavefront driver, but rather calls into the underlying library, @b
-libwavefront_standalone.
+wavefront driver, but rather the underlying planner code.
The planning algorithm assumes that the robot is circular and holonomic.
Under these assumptions, it efficiently dilates obstacles and then plans
@@ -64,7 +53,7 @@
@section usage Usage
@verbatim
-$ wavefront_player
+$ wavefront
@endverbatim
<hr>
@@ -72,30 +61,27 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "localizedpose"/RobotBase2DOdom : robot's map pose. Only the position information is used (velocity is ignored).
-- @b "goal"/robot_msgs/PoseStamped : goal for the robot.
-- @b "scan"/laser_scan/LaserScan : laser scans. Used to temporarily modify the map for dynamic obstacles.
+- @b "tf_message" tf/tfMessage: robot's pose in the "map" frame
+- @b "goal" robot_msgs/PoseStamped : goal for the robot.
+- @b "scan" laser_scan/LaserScan : laser scans. Used to temporarily modify the map for dynamic obstacles.
Publishes to (name / type):
-- @b "cmd_vel"/PoseDot : velocity commands to robot
-- @b "state"/Planner2DState : current planner state (e.g., goal reached, no
-path)
-- @b "gui_path"/Polyline2D : current global path (for visualization)
-- @b "gui_laser"/Polyline2D : re-projected laser scans (for visualization)
+- @b "cmd_vel" robot_msgs/PoseDot : velocity commands to robot
+- @b "state" robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
+- @b "gui_path" robot_msgs/Polyline2D : current global path (for visualization)
+- @b "gui_laser" robot_msgs/Polyline2D : re-projected laser scans (for visualization)
-@todo Start using libTF for transform management:
- - subscribe to odometry and use transform to recover map pose.
-
<hr>
@section parameters ROS parameters
-- None
+ - @b "~dist_eps" (double) : Goal distance tolerance (how close the robot must be to the goal before stopping), default: 1.0 meters
+ - @b "~robot_radius" (double) : The robot's largest radius (the planner treats it as a circle), default: 0.175 meters
+ - @b "~dist_penalty" (double) : Penalty factor for coming close to obstacles, default: 2.0
-@todo There are an enormous number of parameters, values for which are all
-currently hardcoded. These should be exposed as ROS parameters. In
-particular, robot radius and safety distance must be changed for each
-robot.
+@todo There are many more parameters of the underlying planner, values
+for which are currently hardcoded. These should be exposed as ROS
+parameters.
**/
#include <stdio.h>
@@ -243,7 +229,7 @@
#define _xy_tolerance 1e-3
#define _th_tolerance 1e-3
-#define USAGE "USAGE: wavefront_player"
+#define USAGE "USAGE: wavefront"
int
main(int argc, char** argv)
@@ -264,7 +250,7 @@
}
WavefrontNode::WavefrontNode() :
- ros::Node("wavefront_player"),
+ ros::Node("wavefront"),
planner_state(NO_GOAL),
enable(true),
rotate_dir(0),
@@ -294,9 +280,9 @@
///\todo does this need to be initialized? global_pose.setIdentity();
// set a few parameters. leave defaults just as in the ctor initializer list
- param("dist_eps", dist_eps, 1.0);
- param("robot_radius", robot_radius, 0.175);
- param("dist_penalty", dist_penalty, 2.0);
+ param("~dist_eps", dist_eps, 1.0);
+ param("~robot_radius", robot_radius, 0.175);
+ param("~dist_penalty", dist_penalty, 2.0);
// get map via RPC
robot_srvs::StaticMap::Request req;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-04-23 00:32:13
|
Revision: 14300
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14300&view=rev
Author: gerkey
Date: 2009-04-23 00:32:10 +0000 (Thu, 23 Apr 2009)
Log Message:
-----------
Moved SDL tools to sail
Removed Paths:
-------------
pkg/trunk/drivers/cam/uvc_cam/
pkg/trunk/drivers/input/joy_view/
pkg/trunk/drivers/laser/laser_view/
pkg/trunk/nav/nav_view_sdl/
pkg/trunk/util/sdlgl/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X