Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4557] pkg/trunk
|
From: <hsu...@us...> - 2008-09-22 22:10:26
|
Revision: 4557
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4557&view=rev
Author: hsujohnhsu
Date: 2008-09-22 22:10:14 +0000 (Mon, 22 Sep 2008)
Log Message:
-----------
added F3D force feedback groundtruth plugin for fingers.
update P3D to broadcast velocity as well, using new TransformWithRateStamped.msg.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/nav/fake_localization/fake_localization.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/actuators.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups_collision.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/transmissions.xml
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless-fake_localization.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators_headless.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-headless.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators_headless.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators_wg.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators_wg_headless.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" output="screen" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -1,4 +1,5 @@
<launch>
+ <!--master auto="start"/-->
<group name="wg">
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators_wg.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-09-22 22:10:14 UTC (rev 4557)
@@ -14,6 +14,7 @@
rospack_add_library(Ros_Time src/Ros_Time.cc)
rospack_add_library(Ros_PTZ src/Ros_PTZ.cc)
rospack_add_library(P3D src/P3D.cc)
+rospack_add_library(F3D src/F3D.cc)
rospack_add_library(gazebo_actuators src/gazebo_actuators.cpp)
# This target requires adding player to the manifest.xml.
Added: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1,118 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: 3D position interface.
+ * Author: Sachin Chitta and John Hsu
+ * Date: 10 June 2008
+ * SVN: $Id$
+ */
+#ifndef F3D_HH
+#define F3D_HH
+
+#include <gazebo/Controller.hh>
+#include <gazebo/Entity.hh>
+
+#include <ros/node.h>
+#include <std_msgs/Pose3DStamped.h>
+#include <std_msgs/Vector3Stamped.h>
+
+namespace gazebo
+{
+ class PositionIface;
+
+/// \addtogroup gazebo_controller
+ /// \{
+ /** \defgroup F3D f3D
+
+ \brief F3D controller.
+
+ \verbatim
+ <controller:F3D name="controller-name">
+ <interface:actarray name="iface-name"/>
+ </controller:F3D>
+ \endverbatim
+
+ \{
+ */
+
+ /// \brief F3D controller
+ /// This is a controller that simulates a 6 dof position sensor
+ class F3D : public Controller
+ {
+ /// Constructor
+ public: F3D(Entity *parent );
+
+ /// Destructor
+ public: virtual ~F3D();
+
+ /// Load the controller
+ /// \param node XML config node
+ /// \return 0 on success
+ protected: virtual void LoadChild(XMLConfigNode *node);
+
+ /// Init the controller
+ /// \return 0 on success
+ protected: virtual void InitChild();
+
+ /// Update the controller
+ /// \return 0 on success
+ protected: virtual void UpdateChild();
+
+ /// Finalize the controller
+ /// \return 0 on success
+ protected: virtual void FiniChild();
+
+ /// The actarray interface
+ private: PositionIface *myIface;
+
+ /// The parent Model
+ private: Model *myParent;
+
+ private: Body *myBody; //Gazebo/ODE body
+
+
+
+ // added for ros message
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: std_msgs::Vector3Stamped vector3Msg;
+
+ // topic name
+ private: std::string topicName;
+
+ // frame transform name, should match link name "map"
+ private: std::string frameName;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
+ };
+
+/** \} */
+/// \}
+
+
+}
+
+#endif
+
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-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-22 22:10:14 UTC (rev 4557)
@@ -32,6 +32,7 @@
#include <ros/node.h>
#include <std_msgs/Pose3DStamped.h>
+#include <std_msgs/TransformWithRateStamped.h>
namespace gazebo
{
@@ -94,7 +95,7 @@
private: ros::node *rosnode;
// ros message
- private: std_msgs::Pose3DStamped poseMsg;
+ private: std_msgs::TransformWithRateStamped transformMsg;
// topic name
private: std::string topicName;
Added: pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1,154 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/*
+ * Desc: 3D position interface for ground truth.
+ * Author: Sachin Chitta and John Hsu
+ * Date: 1 June 2008
+ * SVN info: $Id$
+ */
+
+#include <gazebo/Global.hh>
+#include <gazebo/XMLConfig.hh>
+#include <gazebo/Model.hh>
+#include <gazebo/HingeJoint.hh>
+#include <gazebo/Body.hh>
+#include <gazebo/SliderJoint.hh>
+#include <gazebo/Simulator.hh>
+#include <gazebo/gazebo.h>
+#include <gazebo/GazeboError.hh>
+#include <gazebo/ControllerFactory.hh>
+#include <gazebo_plugin/F3D.hh>
+
+using namespace gazebo;
+
+GZ_REGISTER_DYNAMIC_CONTROLLER("F3D", F3D);
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+F3D::F3D(Entity *parent )
+ : Controller(parent)
+{
+ this->myParent = dynamic_cast<Model*>(this->parent);
+
+ if (!this->myParent)
+ gzthrow("F3D controller requires a Model as its parent");
+
+ rosnode = ros::g_node; // comes from where?
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // this only works for a single camera.
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in F3D \n");
+ }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+F3D::~F3D()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Load the controller
+void F3D::LoadChild(XMLConfigNode *node)
+{
+ this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]);
+
+ if (!this->myIface)
+ gzthrow("F3D controller requires a Actarray Iface");
+
+ std::string bodyName = node->GetString("bodyName", "", 1);
+ this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
+// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
+
+ this->topicName = node->GetString("topicName", "", 1);
+ this->frameName = node->GetString("frameName", "", 1);
+
+ std::cout << "==== topic name for F3D ======== " << this->topicName << std::endl;
+ rosnode->advertise<std_msgs::Vector3Stamped>(this->topicName);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Initialize the controller
+void F3D::InitChild()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Update the controller
+void F3D::UpdateChild()
+{
+ Vector3 torque;
+ Vector3 force;
+
+ // get force on body
+ force = this->myBody->GetForce();
+
+ // get torque on body
+ torque = this->myBody->GetTorque();
+
+ this->myIface->Lock(1);
+ this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+
+ this->myIface->data->pose.pos.x = force.x;
+ this->myIface->data->pose.pos.y = force.y;
+ this->myIface->data->pose.pos.z = force.z;
+
+ this->myIface->data->pose.roll = torque.x;
+ this->myIface->data->pose.pitch = torque.y;
+ this->myIface->data->pose.yaw = torque.z;
+
+ //FIXME: toque not published, need new message type of new topic name for torque
+ //FIXME: use name space of body id (link name)?
+ this->lock.lock();
+ // copy data into pose message
+ this->vector3Msg.header.frame_id = this->frameName;
+ this->vector3Msg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
+ this->vector3Msg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->vector3Msg.header.stamp.sec) );
+
+ this->vector3Msg.vector.x = force.x;
+ this->vector3Msg.vector.y = force.y;
+ this->vector3Msg.vector.z = force.z;
+
+ // publish to ros
+ rosnode->publish(this->topicName,this->vector3Msg);
+ this->lock.unlock();
+
+ this->myIface->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Finalize the controller
+void F3D::FiniChild()
+{
+ // TODO: will be replaced by global ros node eventually
+ if (rosnode != NULL)
+ {
+ std::cout << "shutdown rosnode in F3D" << std::endl;
+ //ros::fini();
+ rosnode->shutdown();
+ //delete rosnode;
+ }
+}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-22 22:10:14 UTC (rev 4557)
@@ -89,7 +89,7 @@
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);
+ rosnode->advertise<std_msgs::TransformWithRateStamped>(this->topicName);
}
////////////////////////////////////////////////////////////////////////////////
@@ -127,30 +127,38 @@
this->myIface->data->pose.pitch = rot.GetPitch();
this->myIface->data->pose.yaw = rot.GetYaw();
+ // get Rates
+ Vector3 vpos = this->myBody->GetPositionRate(); // get velocity in gazebo frame
+ Quatern vrot = this->myBody->GetRotationRate(); // get velocity in gazebo frame
-
this->lock.lock();
// copy data into pose message
- this->poseMsg.header.frame_id = this->frameName;
- this->poseMsg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
- this->poseMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->poseMsg.header.stamp.sec) );
+ this->transformMsg.header.frame_id = this->frameName;
+ this->transformMsg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
+ this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->transformMsg.header.stamp.sec) );
- this->poseMsg.pose3D.position.x = pos.x;
- this->poseMsg.pose3D.position.y = pos.y;
- this->poseMsg.pose3D.position.z = pos.z;
+ this->transformMsg.transform.translation.x = pos.x;
+ this->transformMsg.transform.translation.y = pos.y;
+ this->transformMsg.transform.translation.z = pos.z;
- this->poseMsg.pose3D.orientation.x = rot.x;
- this->poseMsg.pose3D.orientation.y = rot.y;
- this->poseMsg.pose3D.orientation.z = rot.z;
- this->poseMsg.pose3D.orientation.w = rot.u;
+ 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;
+
+ 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;
+
// publish to ros
- rosnode->publish(this->topicName,this->poseMsg);
+ rosnode->publish(this->topicName,this->transformMsg);
this->lock.unlock();
-
-
-
this->myIface->Unlock();
}
Modified: pkg/trunk/nav/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/nav/fake_localization/fake_localization.cpp 2008-09-22 22:10:14 UTC (rev 4557)
@@ -54,7 +54,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_gazebo_ground_truth"/Pose3DEulerFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth"/Pose3DEulerFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
- @b "initialpose"/Pose2DFloat32 : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
@@ -73,6 +73,7 @@
#include <ros/time.h>
#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/TransformWithRateStamped.h>
#include <std_msgs/Pose3DStamped.h>
#include <std_msgs/ParticleCloud2D.h>
#include <std_msgs/Pose2DFloat32.h>
@@ -99,7 +100,7 @@
param("max_publish_frequency", m_maxPublishFrequency, 0.5);
- subscribe("base_pose_gazebo_ground_truth", m_basePosMsg, &FakeOdomNode::basePosReceived);
+ subscribe("base_pose_ground_truth", m_basePosMsg, &FakeOdomNode::basePosReceived);
subscribe("initialpose", m_iniPos, &FakeOdomNode::initialPoseReceived);
}
@@ -116,7 +117,7 @@
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
- std_msgs::Pose3DStamped m_basePosMsg;
+ std_msgs::TransformWithRateStamped m_basePosMsg;
std_msgs::ParticleCloud2D m_particleCloud;
std_msgs::RobotBase2DOdom m_currentPos;
std_msgs::Pose2DFloat32 m_iniPos;
@@ -141,11 +142,23 @@
//since the msg uses Quaternion use libTF to get yaw
libTF::Pose3D pose;
- pose.setFromMessage(m_basePosMsg.pose3D);
+
+ // FIXME: temp work around during libtf upgrade transition
+ std_msgs::Pose3DStamped messagePose3D;
+ messagePose3D.header = m_basePosMsg.header;
+ messagePose3D.pose3D.position.x = m_basePosMsg.transform.translation.x;
+ messagePose3D.pose3D.position.y = m_basePosMsg.transform.translation.y;
+ messagePose3D.pose3D.position.z = m_basePosMsg.transform.translation.z;
+ messagePose3D.pose3D.orientation.x = m_basePosMsg.transform.rotation.x;
+ messagePose3D.pose3D.orientation.y = m_basePosMsg.transform.rotation.y;
+ messagePose3D.pose3D.orientation.z = m_basePosMsg.transform.rotation.z;
+ messagePose3D.pose3D.orientation.w = m_basePosMsg.transform.rotation.w;
+
+ pose.setFromMessage(messagePose3D.pose3D);
m_currentPos.header = m_basePosMsg.header;
- m_currentPos.pos.x = m_basePosMsg.pose3D.position.x;
- m_currentPos.pos.y = m_basePosMsg.pose3D.position.y;
+ m_currentPos.pos.x = m_basePosMsg.transform.translation.x;
+ m_currentPos.pos.y = m_basePosMsg.transform.translation.y;
m_currentPos.pos.th = pose.getEuler().yaw;
m_currentPos.pos.x += m_iniPos.x;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-09-22 22:10:14 UTC (rev 4557)
@@ -21,12 +21,12 @@
add_custom_target(pr2_gazebo_model_headless ALL
COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators_headless.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_headless.model
DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Test Actuators")
+ COMMENT "Building Gazebo model for PR2 Gazebo Actuators")
add_custom_target(pr2_gazebo_model_nolimit ALL
COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/pr2/gazebo/pr2_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_nolimit.model 1
DEPENDS urdf2gazebo
- COMMENT "Building Gazebo model for PR2 Test Actuators with no limits")
+ COMMENT "Building Gazebo model for PR2 Gazebo Actuators with no limits")
#pr2_arm
add_custom_target(pr2d2 ALL
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/actuators.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/actuators.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/actuators.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1 @@
+link ../actuators.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/actuators.xml
___________________________________________________________________
Added: svn:special
+ *
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -53,7 +53,7 @@
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <frameName>gripper_roll_right_frame</frameName>
+ <frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
@@ -62,22 +62,57 @@
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <frameName>gripper_roll_left_frame</frameName>
+ <frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
+ <updateRate>100.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <frameName>base_frame</frameName>
+ <frameName>map</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>
+ <controller:F3D name="f3d_finger_tip_l_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>finger_tip_l_left</bodyName>
+ <topicName>finger_tip_l_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_left_position"/>
+ </controller:F3D>
+ <controller:F3D name="f3d_finger_tip_r_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>finger_tip_r_left</bodyName>
+ <topicName>finger_tip_r_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_l_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>finger_tip_l_right</bodyName>
+ <topicName>finger_tip_l_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_right_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>finger_tip_r_right</bodyName>
+ <topicName>finger_tip_r_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_right_position"/>
+ </controller:F3D>
+
</verbatim>
</map>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -50,19 +50,19 @@
<!-- P3D for position groundtruth -->
<controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
<topicName>gripper_roll_right_pose_ground_truth</topicName>
- <frameName>gripper_roll_right_frame</frameName>
+ <frameName>map</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
+ <updateRate>1000.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
<topicName>gripper_roll_left_pose_ground_truth</topicName>
- <frameName>gripper_roll_left_frame</frameName>
+ <frameName>map</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
@@ -71,13 +71,48 @@
<updateRate>1000.0</updateRate>
<bodyName>base</bodyName>
<topicName>base_pose_ground_truth</topicName>
- <frameName>base_frame</frameName>
+ <frameName>map</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>
+ <controller:F3D name="f3d_finger_tip_l_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_left</bodyName>
+ <topicName>finger_tip_l_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_left_position"/>
+ </controller:F3D>
+ <controller:F3D name="f3d_finger_tip_r_left_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_left</bodyName>
+ <topicName>finger_tip_r_left_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_left_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_l_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_l_right</bodyName>
+ <topicName>finger_tip_l_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_l_right_position"/>
+ </controller:F3D>
+
+ <controller:F3D name="f3d_finger_tip_r_right_controller" plugin="libF3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>finger_tip_r_right</bodyName>
+ <topicName>finger_tip_r_right_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="f3d_finger_tip_r_right_position"/>
+ </controller:F3D>
+
</verbatim>
</map>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1 @@
+link ../groups.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups.xml
___________________________________________________________________
Added: svn:special
+ *
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups_collision.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups_collision.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1 @@
+link ../groups_collision.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/groups_collision.xml
___________________________________________________________________
Added: svn:special
+ *
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/transmissions.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/transmissions.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/transmissions.xml 2008-09-22 22:10:14 UTC (rev 4557)
@@ -0,0 +1 @@
+link ../transmissions.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/transmissions.xml
___________________________________________________________________
Added: svn:special
+ *
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-09-22 22:07:57 UTC (rev 4556)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-09-22 22:10:14 UTC (rev 4557)
@@ -85,7 +85,8 @@
#include <ros/node.h>
#include <std_msgs/LaserScan.h>
#include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/Pose3DStamped.h>
+#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/Pose3D.h>
#include <std_msgs/BaseVel.h>
#include "rosTF/rosTF.h"
@@ -100,7 +101,7 @@
std_msgs::BaseVel velMsg;
std_msgs::LaserScan laserMsg;
std_msgs::RobotBase2DOdom odomMsg;
- std_msgs::Pose3DStamped groundTruthMsg;
+ std_msgs::TransformWithRateStamped groundTruthMsg;
// A mutex to lock access to fields that are used in message callbacks
ros::thread::mutex lock;
@@ -206,7 +207,7 @@
advertise<std_msgs::LaserScan>("scan",10);
advertise<std_msgs::RobotBase2DOdom>("odom",10);
- advertise<std_msgs::Pose3DStamped>("base_pose_ground_truth",10);
+ advertise<std_msgs::TransformWithRateStamped>("base_pose_ground_truth",10);
subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived, 10);
return(0);
}
@@ -295,7 +296,17 @@
// Note that we correct for Stage's screwed-up coord system.
pose.setFromEuler(gpose.y, -gpose.x, 0.0,
Stg::normalize(gpose.a-M_PI/2.0), 0.0, 0.0);
- this->groundTruthMsg.pose3D = pose.getMessage();
+
+ // FIXME: temp work around during libtf upgrade transition
+ std_msgs::Pose3D tmpPose3D = pose.getMessage();
+ this->groundTruthMsg.transform.translation.x = tmpPose3D.position.x;
+ this->groundTruthMsg.transform.translation.y = tmpPose3D.position.y;
+ this->groundTruthMsg.transform.translation.z = tmpPose3D.position.z;
+ this->groundTruthMsg.transform.rotation.x = tmpPose3D.orientation.x;
+ this->groundTruthMsg.transform.rotation.y = tmpPose3D.orientation.y;
+ this->groundTruthMsg.transform.rotation.z = tmpPose3D.orientation.z;
+ this->groundTruthMsg.transform.rotation.w = tmpPose3D.orientation.w;
+
this->groundTruthMsg.header.frame_id = "odom";
this->groundTruthMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
