Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4143] pkg/trunk
|
From: <hsu...@us...> - 2008-09-10 19:49:47
|
Revision: 4143
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4143&view=rev
Author: hsujohnhsu
Date: 2008-09-10 19:49:56 +0000 (Wed, 10 Sep 2008)
Log Message:
-----------
* fixed PTZ initialization command for pan/tilt
* update PTZ axis in pr2.xml so it matches axis camera.
* added offsets for pose in P3D, allows initialization of robot location for fake localization. updates to pr2.xml accordingly <xyzOffsets> <rpyOffsets> tags.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/manifest.xml
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/src/Ros_PTZ.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -11,7 +11,7 @@
<depend package="opende" />
<depend package="ogre" />
<export>
- <cpp lflags="`xml2-config --libs` -Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="`xml2-config --cflags` -I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
+ <cpp lflags="`xml2-config --libs` -Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="`xml2-config --cflags` -I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/controllers -I${prefix}/gazebo-svn/server/controllers/camera/generic -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/Gazebo-manual-svn-html/"/>
</export>
<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-10 19:49:56 UTC (rev 4143)
@@ -103,6 +103,10 @@
// FIXME: extract link name directly?
private: std::string frameName;
+ // allow specifying constant xyz and rpy offsets
+ private: Vector3 xyzOffsets;
+ private: Vector3 rpyOffsets;
+
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-10 19:49:56 UTC (rev 4143)
@@ -85,6 +85,8 @@
this->topicName = node->GetString("topicName", "", 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));
std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
rosnode->advertise<std_msgs::Pose3DStamped>(this->topicName);
@@ -102,11 +104,20 @@
{
Quatern rot;
Vector3 pos;
+
+ // apply xyz offsets
+ pos = this->myBody->GetPosition() + this->xyzOffsets;
+
+ // apply rpy offsets
+ Vector3 rpyTotal;
+ rot = this->myBody->GetRotation();
+ rpyTotal.x = this->rpyOffsets.x + rot.GetRoll();
+ rpyTotal.y = this->rpyOffsets.y + rot.GetPitch();
+ rpyTotal.z = this->rpyOffsets.z + rot.GetYaw();
+ rot.SetFromEuler(rpyTotal);
+
this->myIface->Lock(1);
this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
-
- pos = this->myBody->GetPosition();
- rot = this->myBody->GetRotation();
this->myIface->data->pose.pos.x = pos.x;
this->myIface->data->pose.pos.y = pos.y;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-10 19:49:56 UTC (rev 4143)
@@ -162,6 +162,9 @@
// Initialize the controller
void Ros_PTZ::InitChild()
{
+ // initialize pan/tilt angles
+ this->cmdPan = 0.0;
+ this->cmdTilt = 0.0;
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -75,6 +75,8 @@
<bodyName>base</bodyName>
<topicName>base_pose</topicName>
<frameName>base_frame</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 17:57:59 UTC (rev 4142)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-09-10 19:49:56 UTC (rev 4143)
@@ -826,22 +826,22 @@
<anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
</joint>
<joint name="ptz_pan_left_joint" type="revolute">
- <axis xyz=" 0 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 0 -1 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_pan_min_limit" max="ptz_pan_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_tilt_left_joint" type="revolute">
- <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 -1 0 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_tilt_min_limit" max="ptz_tilt_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_pan_right_joint" type="revolute">
- <axis xyz=" 0 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 0 -1 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_pan_min_limit" max="ptz_pan_max_limit" effort="10" velocity="5" />
</joint>
<joint name="ptz_tilt_right_joint" type="revolute">
- <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate frame -->
+ <axis xyz=" 0 -1 0 " /> <!-- direction of the joint in a local coordinate frame, axis ptz uses opposite sign than our robot frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<limit min=" ptz_tilt_min_limit" max="ptz_tilt_max_limit" effort="10" velocity="5" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
