Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[6519] pkg/trunk
|
From: <hsu...@us...> - 2008-11-11 08:23:53
|
Revision: 6519
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6519&view=rev
Author: hsujohnhsu
Date: 2008-11-11 08:23:50 +0000 (Tue, 11 Nov 2008)
Log Message:
-----------
* rosstage, fake_localization, P3D now uses PoseWithRatesStamped. No more TransformWithRateStamped.
* update gazebo_plugin tests to use new P3D message PoseWithRatesStamped.
* pr2_obs.launch geneates pr2_xml.model.
* remove IMUTopicName and use topicName in P3D xml specifications.
* add gaussian noise for robot_floorobj.world, P3D plugins.
* update pr2_robot_defs/head_defs.xml to use better inertia properties for tilting hokuyo.
Modified Paths:
--------------
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml $(find gazebo_robot_description)/world/pr2_xml_dual_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/dual_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/dual_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml $(find gazebo_robot_description)/world/pr2_xml_multi_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/multi_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/multi_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/multi_link_test/pr2_multi_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_multi_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,7 +3,8 @@
<!-- create model file for gazebo -->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/single_link_test/pr2_single_link.xml $(find gazebo_robot_description)/world/pr2_xml_single_link.model" />
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/single_link_test/send_description.xml" />
+ <!--include file="$(find wg_robot_description)/single_link_test/send_description.xml" /-->
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/single_link_test/pr2_single_link.xml"" />
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-11-11 08:23:50 UTC (rev 6519)
@@ -1,5 +1,7 @@
<launch>
<group name="wg">
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml.model" />
+ <node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2/pr2.xml $(find gazebo_robot_description)/world/pr2_xml_nolimit.model 1" />
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_obstacle.world" respawn="false">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1.launch 2008-11-11 08:23:50 UTC (rev 6519)
@@ -3,11 +3,10 @@
<!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
<!-- create model file for gazebo -->
- <!--node pkg="xacro" type="xacro.py" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml $(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded" /-->
<node pkg="gazebo_robot_description" type="urdf2gazebo" args="$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded $(find gazebo_robot_description)/world/pr2_xml_prototype1.model" />
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xml.expanded"" />
+ <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot_prototype1.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-11-11 08:23:50 UTC (rev 6519)
@@ -53,7 +53,6 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
- <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -78,7 +77,6 @@
<updateRate>1000.0</updateRate>
<bodyName>body_name</bodyName>
<topicName>body_pose_ground_truth</topicName>
- <IMUTopicName>body_pose_IMU</IMUTopicName>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
@@ -122,12 +120,10 @@
private: ros::node *rosnode;
/// \brief ros message
- private: std_msgs::TransformWithRateStamped transformMsg;
private: std_msgs::PoseWithRatesStamped poseMsg;
/// \brief topic name
private: std::string topicName;
- private: std::string IMUTopicName;
/// \brief frame transform name, should match link name
/// FIXME: extract link name directly?
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-11-11 08:23:50 UTC (rev 6519)
@@ -79,16 +79,14 @@
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
this->topicName = node->GetString("topicName", "ground_truth", 1);
- this->IMUTopicName = node->GetString("IMUTopicName", "", 1);
this->frameName = node->GetString("frameName", "", 1);
this->xyzOffsets = node->GetVector3("xyzOffsets", Vector3(0,0,0));
this->rpyOffsets = node->GetVector3("rpyOffsets", Vector3(0,0,0));
this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
- rosnode->advertise<std_msgs::TransformWithRateStamped>(this->topicName,10);
- if (this->IMUTopicName != "")
- rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->IMUTopicName,10);
+ if (this->topicName != "")
+ rosnode->advertise<std_msgs::PoseWithRatesStamped>(this->topicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -138,46 +136,7 @@
this->lock.lock();
- /// @todo: remove transform message
- // copy data into a transform message
- this->transformMsg.header.frame_id = this->frameName;
- this->transformMsg.header.stamp.sec = (unsigned long)floor(cur_time);
- this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( cur_time - this->transformMsg.header.stamp.sec) );
-
- this->transformMsg.transform.translation.x = pos.x;
- this->transformMsg.transform.translation.y = pos.y;
- this->transformMsg.transform.translation.z = pos.z;
-
- this->transformMsg.transform.rotation.x = rot.x;
- this->transformMsg.transform.rotation.y = rot.y;
- this->transformMsg.transform.rotation.z = rot.z;
- this->transformMsg.transform.rotation.w = rot.u;
-
- this->transformMsg.rate.translation.x = vpos.x;
- this->transformMsg.rate.translation.y = vpos.y;
- this->transformMsg.rate.translation.z = vpos.z;
-
- // pass quaternion
- // this->transformMsg.rate.rotation.x = vrot.x;
- // this->transformMsg.rate.rotation.y = vrot.y;
- // this->transformMsg.rate.rotation.z = vrot.z;
- // this->transformMsg.rate.rotation.w = vrot.u;
-
- // pass euler anglular rates
- this->transformMsg.rate.rotation.x = veul.x;
- this->transformMsg.rate.rotation.y = veul.y;
- this->transformMsg.rate.rotation.z = veul.z;
- this->transformMsg.rate.rotation.w = 0;
-
- // publish to ros
- rosnode->publish(this->topicName,this->transformMsg);
-
-
-
-
-
-
- if (this->IMUTopicName != "")
+ if (this->topicName != "")
{
// copy data into pose message
this->poseMsg.header.frame_id = "map"; // @todo: should this be changeable?
@@ -212,7 +171,7 @@
this->poseMsg.acc.ang_acc.az = this->aeul.z + this->GaussianKernel(0,this->gaussianNoise) ;
// publish to ros
- rosnode->publish(this->IMUTopicName,this->poseMsg);
+ rosnode->publish(this->topicName,this->poseMsg);
}
this->lock.unlock();
@@ -225,9 +184,8 @@
// Finalize the controller
void P3D::FiniChild()
{
- rosnode->unadvertise(this->topicName);
- if (this->IMUTopicName != "")
- rosnode->unadvertise(this->IMUTopicName);
+ if (this->topicName != "")
+ rosnode->unadvertise(this->topicName);
}
//////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testbase.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -69,26 +69,25 @@
def printBase(self, p3d):
- print "P3D pose translan: " + "x: " + str(p3d.transform.translation.x)
- print " " + "y: " + str(p3d.transform.translation.y)
- print " " + "z: " + str(p3d.transform.translation.z)
- print "P3D pose rotation: " + "x: " + str(p3d.transform.rotation.x)
- print " " + "y: " + str(p3d.transform.rotation.y)
- print " " + "z: " + str(p3d.transform.rotation.z)
- print " " + "w: " + str(p3d.transform.rotation.w)
- print "P3D rate translan: " + "x: " + str(p3d.rate.translation.x)
- print " " + "y: " + str(p3d.rate.translation.y)
- print " " + "z: " + str(p3d.rate.translation.z)
- print "P3D rate rotation: " + "x: " + str(p3d.rate.rotation.x)
- print " " + "y: " + str(p3d.rate.rotation.y)
- print " " + "z: " + str(p3d.rate.rotation.z)
- print " " + "w: " + str(p3d.rate.rotation.w)
+ print "P3D pose translan: " + "x: " + str(p3d.pos.position.x)
+ print " " + "y: " + str(p3d.pos.position.y)
+ print " " + "z: " + str(p3d.pos.position.z)
+ print "P3D pose rotation: " + "x: " + str(p3d.pos.orientation.x)
+ print " " + "y: " + str(p3d.pos.orientation.y)
+ print " " + "z: " + str(p3d.pos.orientation.z)
+ print " " + "w: " + str(p3d.pos.orientation.w)
+ print "P3D rate translan: " + "x: " + str(p3d.vel.vel.x)
+ print " " + "y: " + str(p3d.vel.vel.y)
+ print " " + "z: " + str(p3d.vel.vel.z)
+ print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
+ print " " + "y: " + str(p3d.vel.ang_vel.vy)
+ print " " + "z: " + str(p3d.vel.ang_vel.vz)
def p3dInput(self, p3d):
i = 0
print "base pose ground truth received"
self.printBase(p3d)
- error = abs(p3d.rate.rotation.z - TARGET_VW)
+ error = abs(p3d.vel.ang_vel.vz - TARGET_VW)
print " Error: " + str(error)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_vw:
@@ -107,7 +106,7 @@
def test_base(self):
print "LNK\n"
pub = rospy.Publisher("cmd_vel", BaseVel)
- rospy.Subscriber("base_pose_ground_truth", TransformWithRateStamped, self.p3dInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 60.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testpendulum.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -106,27 +106,26 @@
#def printPendulum(self, p3d):
- #print "P3D pose translan: " + "x: " + str(p3d.transform.translation.x)
- #print " " + "y: " + str(p3d.transform.translation.y)
- #print " " + "z: " + str(p3d.transform.translation.z)
- #print "P3D pose rotation: " + "x: " + str(p3d.transform.rotation.x)
- #print " " + "y: " + str(p3d.transform.rotation.y)
- #print " " + "z: " + str(p3d.transform.rotation.z)
- #print " " + "w: " + str(p3d.transform.rotation.w)
- #print "P3D rate translan: " + "x: " + str(p3d.rate.translation.x)
- #print " " + "y: " + str(p3d.rate.translation.y)
- #print " " + "z: " + str(p3d.rate.translation.z)
- #print "P3D rate rotation: " + "x: " + str(p3d.rate.rotation.x)
- #print " " + "y: " + str(p3d.rate.rotation.y)
- #print " " + "z: " + str(p3d.rate.rotation.z)
- #print " " + "w: " + str(p3d.rate.rotation.w)
+ #print "P3D pose translan: " + "x: " + str(p3d.pos.position.x)
+ #print " " + "y: " + str(p3d.pos.position.y)
+ #print " " + "z: " + str(p3d.pos.position.z)
+ #print "P3D pose rotation: " + "x: " + str(p3d.pos.orientation.x)
+ #print " " + "y: " + str(p3d.pos.orientation.y)
+ #print " " + "z: " + str(p3d.pos.orientation.z)
+ #print " " + "w: " + str(p3d.pos.orientation.w)
+ #print "P3D rate translan: " + "x: " + str(p3d.vel.vel.x)
+ #print " " + "y: " + str(p3d.vel.vel.y)
+ #print " " + "z: " + str(p3d.vel.vel.z)
+ #print "P3D rate rotation: " + "x: " + str(p3d.vel.ang_vel.vx)
+ #print " " + "y: " + str(p3d.vel.ang_vel.vy)
+ #print " " + "z: " + str(p3d.vel.ang_vel.vz)
def p3dInput1(self, p3d):
#print "link1 pose ground truth received"
#self.printPendulum(p3d)
- tmpx = p3d.transform.translation.x
- tmpz = p3d.transform.translation.z - 2.0
+ tmpx = p3d.pos.position.x
+ tmpz = p3d.pos.position.z - 2.0
#print "link1 origin (" + str(tmpx) + " , " + str(tmpz) + ")"
self.error1_total += math.sqrt(tmpx*tmpx+tmpz*tmpz)
self.error1_count += 1
@@ -136,19 +135,19 @@
def p3dInput2(self, p3d):
#print "link2 pose ground truth received"
#self.printPendulum(p3d)
- q = Q(p3d.transform.rotation.x , p3d.transform.rotation.y , p3d.transform.rotation.z , p3d.transform.rotation.w)
+ q = Q(p3d.pos.orientation.x , p3d.pos.orientation.y , p3d.pos.orientation.z , p3d.pos.orientation.w)
q.normalize()
v = q.getEuler()
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- #FIXME: something wrong with the transform, need to fix it. abs masks the problem for now.
- tmpx = abs(p3d.transform.translation.x) +0.0 - abs(math.cos(v.z)*math.cos(v.y))
- tmpz = abs(p3d.transform.translation.z) -2.0 + abs(math.sin(v.y))
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ #FIXME: something wrong with the pos, need to fix it. abs masks the problem for now.
+ tmpx = abs(p3d.pos.position.x) +0.0 - abs(math.cos(v.z)*math.cos(v.y))
+ tmpz = abs(p3d.pos.position.z) -2.0 + abs(math.sin(v.y))
#math.cos(v.x)*math.cos(v.z)-math.cos(v.x)*math.sin(v.y)*math.cos(v.z))
#print "link2 origin (" + str(tmpx) + " , " + str(tmpz) + ")"
- #print "link2 raw (" + str(p3d.transform.translation.x) + " , " + str(p3d.transform.translation.z) + ") total: " + str(self.error2_total)
+ #print "link2 raw (" + str(p3d.pos.position.x) + " , " + str(p3d.pos.position.z) + ") total: " + str(self.error2_total)
#print "link2 correc (" + str(math.cos(v.y)) + " , " + str(math.sin(v.y)) + ") angle: " + str(v.x) +","+str(v.y)+","+str(v.z)
self.error2_total += math.sqrt(tmpx*tmpx+tmpz*tmpz)
@@ -158,8 +157,8 @@
def test_pendulum(self):
print "LNK\n"
- rospy.Subscriber("link1_pose", TransformWithRateStamped, self.p3dInput1)
- rospy.Subscriber("link2_pose", TransformWithRateStamped, self.p3dInput2)
+ rospy.Subscriber("link1_pose", PoseWithRatesStamped, self.p3dInput1)
+ rospy.Subscriber("link2_pose", PoseWithRatesStamped, self.p3dInput2)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 20.0
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/testslide.py 2008-11-11 08:23:50 UTC (rev 6519)
@@ -62,26 +62,26 @@
self.hits = 0
self.runs = 0
- def positionInput(self, pose):
+ def positionInput(self, p3d):
self.runs = self.runs + 1
- print " got pose ", self.runs
+ print " got p3d ", self.runs
#if (pos.frame == 1):
- print "x ", pose.transform.translation.x
- print "y ", pose.transform.translation.y
- print "z ", pose.transform.translation.z
- dx = pose.transform.translation.x - TARGET_X
- dy = pose.transform.translation.y - TARGET_Y
- dz = pose.transform.translation.z - TARGET_Z
+ print "x ", p3d.pos.position.x
+ print "y ", p3d.pos.position.y
+ print "z ", p3d.pos.position.z
+ dx = p3d.pos.position.x - TARGET_X
+ dy = p3d.pos.position.y - TARGET_Y
+ dz = p3d.pos.position.z - TARGET_Z
d = math.sqrt((dx * dx) + (dy * dy)) #+ (dz * dz))
- print "P: " + str(pose.transform.translation.x) + " " + str(pose.transform.translation.y)
+ print "P: " + str(p3d.pos.position.x) + " " + str(p3d.pos.position.y)
#print "D: " + str(dx) + " " + str(dy) + " " + str(dz) + " " + str(d) + " < " + str(TARGET_RAD * TARGET_RAD)
if (d < TARGET_RAD):
- #print "HP: " + str(dx) + " " + str(dy) + " " + str(d) + " at " + str(pos.transform.translation.x) + " " + str(pos.transform.translation.y)
+ #print "HP: " + str(dx) + " " + str(dy) + " " + str(d) + " at " + str(p3d.pos.position.x) + " " + str(p3d.pos.position.y)
#print "DONE"
self.hits = self.hits + 1
print "Hit goal, " + str(self.hits)
if (self.runs < 100 and self.runs > 10):
- print "Obviously wrong transforms!"
+ print "Obviously wrong poses!"
self.success = False
self.fail = True
#os.system("killall gazebo")
@@ -95,7 +95,7 @@
def testslide(self):
print "LINK\n"
#rospy.Subscriber("Odom", RobotBase2DOdom, self.positionInput)
- rospy.Subscriber("base_pose_ground_truth", TransformWithRateStamped, self.positionInput)
+ rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.positionInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + 50.0 #59 seconds
while not rospy.is_shutdown() and not self.success and not self.fail and time.time() < timeout_t:
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-11-11 08:23:50 UTC (rev 6519)
@@ -54,7 +54,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth"/std_msgs::TransformWithRateStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth"/std_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).
Publishes to (name / type):
@@ -73,7 +73,7 @@
#include <ros/time.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/PoseWithRatesStamped.h>
#include <std_msgs/Pose3DStamped.h>
#include <std_msgs/ParticleCloud2D.h>
#include <std_msgs/Pose2DFloat32.h>
@@ -117,7 +117,7 @@
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
- std_msgs::TransformWithRateStamped m_basePosMsg;
+ std_msgs::PoseWithRatesStamped m_basePosMsg;
std_msgs::ParticleCloud2D m_particleCloud;
std_msgs::RobotBase2DOdom m_currentPos;
std_msgs::Pose2DFloat32 m_iniPos;
@@ -140,12 +140,12 @@
m_lastUpdate = ros::Time::now();
- tf::Transform txi(tf::Quaternion(m_basePosMsg.transform.rotation.x,
- m_basePosMsg.transform.rotation.y,
- m_basePosMsg.transform.rotation.z,
- m_basePosMsg.transform.rotation.w),
- tf::Point(m_basePosMsg.transform.translation.x,
- m_basePosMsg.transform.translation.y, 0.0));
+ tf::Transform txi(tf::Quaternion(m_basePosMsg.pos.orientation.x,
+ m_basePosMsg.pos.orientation.y,
+ m_basePosMsg.pos.orientation.z,
+ m_basePosMsg.pos.orientation.w),
+ tf::Point(m_basePosMsg.pos.position.x,
+ m_basePosMsg.pos.position.y, 0.0));
double x = txi.getOrigin().x() + m_iniPos.x;
double y = txi.getOrigin().y() + m_iniPos.y;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -333,7 +333,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
@@ -344,7 +343,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
@@ -355,7 +353,6 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <IMUTopicName>base_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -354,7 +354,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -364,7 +364,7 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -374,7 +374,7 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <IMUTopicName>base_pose_imu</IMUTopicName>
+ <gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
<rpyOffsets>0 0 0</rpyOffsets>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_prototype1.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -145,7 +145,6 @@
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <IMUTopicName>base_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world 2008-11-11 08:23:50 UTC (rev 6519)
@@ -193,7 +193,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_right_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
@@ -204,7 +203,6 @@
<updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <IMUTopicName>gripper_roll_left_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
@@ -215,7 +213,6 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <IMUTopicName>base_pose_imu</IMUTopicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/send_description.launch 2008-11-11 08:23:50 UTC (rev 6519)
@@ -1,5 +1,5 @@
<launch>
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2_new.xml'" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xml'" />
</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/head_defs.xml 2008-11-11 08:23:50 UTC (rev 6519)
@@ -13,7 +13,7 @@
<anchor xyz="0 0 0" />
<limit min="-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
<calibration reference_position="-0.35" values="0 0" />
- <joint_properties damping="1.0" />
+ <joint_properties damping="0.1" />
</joint>
<link name="${name}_mount">
@@ -21,9 +21,9 @@
<insert_block name="origin" />
<joint name="${name}_mount_joint" />
<inertial>
- <mass value="1.0" />
+ <mass value="0.01" />
<com xyz="0 0 0" />
- <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
@@ -62,7 +62,7 @@
<inertial>
<mass value="0.1" />
<com xyz="0 0 0" />
- <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
+ <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
</inertial>
<visual>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-11-11 08:12:39 UTC (rev 6518)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-11-11 08:23:50 UTC (rev 6519)
@@ -85,7 +85,7 @@
#include <ros/node.h>
#include <std_msgs/LaserScan.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/PoseWithRatesStamped.h>
#include <std_msgs/Pose3D.h>
#include <std_msgs/BaseVel.h>
@@ -101,7 +101,7 @@
std_msgs::BaseVel velMsg;
std_msgs::LaserScan laserMsg;
std_msgs::RobotBase2DOdom odomMsg;
- std_msgs::TransformWithRateStamped groundTruthMsg;
+ std_msgs::PoseWithRatesStamped groundTruthMsg;
// A mutex to lock access to fields that are used in message callbacks
ros::thread::mutex lock;
@@ -210,7 +210,7 @@
advertise<std_msgs::LaserScan>("base_scan",10);
advertise<std_msgs::RobotBase2DOdom>("odom",10);
- advertise<std_msgs::TransformWithRateStamped>("base_pose_ground_truth",10);
+ advertise<std_msgs::PoseWithRatesStamped>("base_pose_ground_truth",10);
subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived, 10);
return(0);
}
@@ -289,13 +289,13 @@
tf::Transform gt(tf::Quaternion(gpose.a-M_PI/2.0, 0, 0),
tf::Point(gpose.y, -gpose.x, 0.0));
- this->groundTruthMsg.transform.translation.x = gt.getOrigin().x();
- this->groundTruthMsg.transform.translation.y = gt.getOrigin().y();
- this->groundTruthMsg.transform.translation.z = gt.getOrigin().z();
- this->groundTruthMsg.transform.rotation.x = gt.getRotation().x();
- this->groundTruthMsg.transform.rotation.y = gt.getRotation().y();
- this->groundTruthMsg.transform.rotation.z = gt.getRotation().z();
- this->groundTruthMsg.transform.rotation.w = gt.getRotation().w();
+ this->groundTruthMsg.pos.position.x = gt.getOrigin().x();
+ this->groundTruthMsg.pos.position.y = gt.getOrigin().y();
+ this->groundTruthMsg.pos.position.z = gt.getOrigin().z();
+ this->groundTruthMsg.pos.orientation.x = gt.getRotation().x();
+ this->groundTruthMsg.pos.orientation.y = gt.getRotation().y();
+ this->groundTruthMsg.pos.orientation.z = gt.getRotation().z();
+ this->groundTruthMsg.pos.orientation.w = gt.getRotation().w();
this->groundTruthMsg.header.frame_id = "odom";
this->groundTruthMsg.header.stamp = sim_time;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
