Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3675] pkg/trunk
|
From: <hsu...@us...> - 2008-08-27 03:25:07
|
Revision: 3675
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3675&view=rev
Author: hsujohnhsu
Date: 2008-08-27 03:25:16 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
update P3D plugins to broadcast message with pose informations in robot_msgs/Pose3DEulerFloat32.
updated xmls to reflect change (added topic names and frame names).
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -31,6 +31,9 @@
#include <gazebo/Entity.hh>
#include <pr2Core/pr2Core.h>
+#include <ros/node.h>
+#include <robot_msgs/Pose3DEulerFloat32.h>
+
namespace gazebo
{
class PositionIface;
@@ -84,6 +87,26 @@
private: Model *myParent;
private: Body *myBody; //Gazebo/ODE body
+
+
+
+ // added for ros message
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: robot_msgs::Pose3DEulerFloat32 poseMsg;
+
+ // topic name
+ private: std::string topicName;
+
+ // frame transform name, should match link name
+ // FIXME: extract link name directly?
+ private: std::string frameName;
+
+ // 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/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -27,9 +27,6 @@
#ifndef ROS_CAMERA_HH
#define ROS_CAMERA_HH
-#include <rosTF/rosTF.h>
-
-
#include <ros/node.h>
#include <std_msgs/Image.h>
#include <gazebo/Controller.hh>
@@ -118,9 +115,6 @@
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
- // transform server
- private: rosTFClient *tfc;
-
};
/** /} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -32,7 +32,6 @@
#include <ros/node.h>
#include <std_msgs/LaserScan.h>
-#include <rosTF/rosTF.h>
namespace gazebo
{
@@ -128,9 +127,6 @@
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
- // transform server
- private: rosTFClient *tfc;
-
};
/** /} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -20,7 +20,7 @@
<depend package="rosTF"/>
<depend package="libTF"/>
<depend package="rosthread"/>
-<depend package="std_msgs"/>
+<depend package="robot_msgs"/>
<depend package="stl_utils" />
<depend package="math_utils" />
<depend package="gazebo_robot_description"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-08-27 03:25:16 UTC (rev 3675)
@@ -51,6 +51,17 @@
if (!this->myParent)
gzthrow("P3D 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 P3D \n");
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -71,6 +82,12 @@
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 P3D ======== " << this->topicName << std::endl;
+ rosnode->advertise<robot_msgs::Pose3DEulerFloat32>(this->topicName);
}
////////////////////////////////////////////////////////////////////////////////
@@ -99,6 +116,29 @@
this->myIface->data->pose.pitch = rot.GetPitch();
this->myIface->data->pose.yaw = rot.GetYaw();
+
+
+ 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->poseMsg.position.x = pos.x;
+ this->poseMsg.position.y = pos.y;
+ this->poseMsg.position.z = pos.z;
+
+ this->poseMsg.orientation.roll = rot.GetRoll();
+ this->poseMsg.orientation.pitch = rot.GetPitch();
+ this->poseMsg.orientation.yaw = rot.GetYaw();
+
+ // publish to ros
+ rosnode->publish(this->topicName,this->poseMsg);
+ this->lock.unlock();
+
+
+
+
this->myIface->Unlock();
}
@@ -106,4 +146,12 @@
// Finalize the controller
void P3D::FiniChild()
{
+ // TODO: will be replaced by global ros node eventually
+ if (rosnode != NULL)
+ {
+ std::cout << "shutdown rosnode in P3D" << std::endl;
+ //ros::fini();
+ rosnode->shutdown();
+ //delete rosnode;
+ }
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -42,18 +42,24 @@
<controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose</topicName>
+ <frameName>gripper_roll_right_frame</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
+ <topicName>base_pose</topicName>
+ <frameName>base_frame</frameName>
<interface:position name="p3d_base_position"/>
</controller:P3D>
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-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -48,18 +48,24 @@
<controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose</topicName>
+ <frameName>gripper_roll_right_frame</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
+ <topicName>base_pose</topicName>
+ <frameName>base_frame</frameName>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -33,6 +33,8 @@
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
Added: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg 2008-08-27 03:25:16 UTC (rev 3675)
@@ -0,0 +1,3 @@
+Header header
+std_msgs/Point3DFloat32 position
+std_msgs/EulerAnglesFloat32 orientation
Added: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg 2008-08-27 03:25:16 UTC (rev 3675)
@@ -0,0 +1,3 @@
+Header header
+std_msgs/Point3DFloat64 position
+std_msgs/EulerAnglesFloat64 orientation
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
