[Personalrobots-commit] SF.net SVN: personalrobots:[3868] pkg/trunk

[<hsu...@us...>]

Revision: 3868
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3868&view=rev
Author:   hsujohnhsu
Date:     2008-09-02 18:33:55 +0000 (Tue, 02 Sep 2008)

Log Message:
-----------
fix to pr2.xml ptz tilt axis joint.
update ptz controller to use ros controls through wx_camera_panel.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
    pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
    pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
    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/controllers_gazebo_test_headless.xml
    pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt	2008-09-02 18:33:55 UTC (rev 3868)
@@ -12,6 +12,7 @@
 rospack_add_library(Ros_Camera src/Ros_Camera.cc)
 rospack_add_library(Ros_Laser src/Ros_Laser.cc)
 rospack_add_library(Ros_Node src/Ros_Node.cc)
+rospack_add_library(Ros_PTZ src/Ros_PTZ.cc)
 rospack_add_library(P3D src/P3D.cc)
 rospack_add_library(gazebo_actuators src/gazebo_actuators.cpp)
 rospack_add_library(test_actuators src/test_actuators.cpp)

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh	2008-09-02 18:33:55 UTC (rev 3868)
@@ -31,6 +31,13 @@
 #include <gazebo/Param.hh>
 #include <gazebo/Controller.hh>
 
+// ros messages
+#include <ros/node.h>
+
+// messages for controlling ptz
+#include <axis_cam/PTZActuatorState.h>
+#include <axis_cam/PTZActuatorCmd.h>
+
 namespace gazebo
 {
   class HingeJoint;
@@ -112,6 +119,30 @@
 
     private: Param<std::string> *panJointNameP;
     private: Param<std::string> *tiltJointNameP;
+    private: Param<std::string> *commandTopicNameP;
+    private: Param<std::string> *stateTopicNameP;
+
+    // pointer to ros node
+    private: ros::node *rosnode;
+    // ros message
+    private: axis_cam::PTZActuatorState PTZStateMessage;
+    private: axis_cam::PTZActuatorCmd   PTZControlMessage;
+
+    // receive message
+    private: void PTZCommandReceived();
+
+    // topic name
+    private: std::string commandTopicName;
+    private: std::string stateTopicName;
+
+    // frame transform name, should match link name
+    // FIXME: extract link name directly? currently using joint names
+    private: std::string panFrameName;
+    private: std::string tiltFrameName;
+
+    // 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/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml	2008-09-02 18:33:55 UTC (rev 3868)
@@ -27,6 +27,7 @@
 <depend package="mechanism_control" />
 <depend package="wg_robot_description_parser" />
 <depend package="tinyxml" />
+<depend package="axis_cam" />
 <export>
 <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib" />
 </export>

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc	2008-09-02 18:33:55 UTC (rev 3868)
@@ -36,11 +36,12 @@
 #include <gazebo/GazeboError.hh>
 #include <gazebo/ControllerFactory.hh>
 #include <gazebo/HingeJoint.hh>
-#include <gazebo/Ros_PTZ.hh>
 
+#include <gazebo_plugin/Ros_PTZ.hh>
+
 using namespace gazebo;
 
-GZ_REGISTER_STATIC_CONTROLLER("ros_ptz", Ros_PTZ);
+GZ_REGISTER_DYNAMIC_CONTROLLER("Ros_PTZ", Ros_PTZ);
 
 ////////////////////////////////////////////////////////////////////////////////
 // Constructor
@@ -62,6 +63,20 @@
   this->tiltJointNameP = new Param<std::string>("tiltJoint", "", 1);
   this->motionGainP = new Param<double>("motionGain",2,0);
   this->forceP = new Param<double>("force",10,0);
+  this->commandTopicNameP = new Param<std::string>("commandTopicName","PTZ_cmd",0);
+  this->stateTopicNameP = new Param<std::string>("stateTopicName","PTZ_state",0);
+
+  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 camera \n");
+  }
+
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -80,6 +95,8 @@
   delete this->tiltJointNameP;
   delete this->motionGainP;
   delete this->forceP;
+  delete this->commandTopicNameP;
+  delete this->stateTopicNameP;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -95,10 +112,24 @@
   this->tiltJointNameP->Load(node);
   this->motionGainP->Load(node);
   this->forceP->Load(node);
+  this->commandTopicNameP->Load(node);
+  this->stateTopicNameP->Load(node);
 
   this->panJoint = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->panJointNameP->GetValue()));
   this->tiltJoint = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(this->tiltJointNameP->GetValue()));
 
+  this->commandTopicName = this->commandTopicNameP->GetValue();
+  this->stateTopicName = this->stateTopicNameP->GetValue();
+
+  this->panFrameName = this->panJointNameP->GetValue();
+  this->tiltFrameName = this->tiltJointNameP->GetValue();
+
+  std::cout << " publishing state topic for ptz " << this->stateTopicName << std::endl;
+  std::cout << " subscribing command topic for ptz " << this->commandTopicName << std::endl;
+
+  rosnode->advertise<axis_cam::PTZActuatorState>(this->stateTopicName);
+  rosnode->subscribe( commandTopicName, PTZControlMessage, &Ros_PTZ::PTZCommandReceived,this);
+
   if (!this->panJoint)
     gzthrow("couldn't get pan hinge joint");
 
@@ -107,6 +138,14 @@
 
 }
 
+void Ros_PTZ::PTZCommandReceived()
+{
+  this->lock.lock();
+  this->cmdPan  = PTZControlMessage.pan.cmd*M_PI/180.0;
+  this->cmdTilt = PTZControlMessage.tilt.cmd*M_PI/180.0;
+  this->lock.unlock();
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 /// Save the controller.
 void Ros_PTZ::SaveChild(std::string &prefix, std::ostream &stream)
@@ -134,13 +173,14 @@
 // Update the controller
 void Ros_PTZ::UpdateChild()
 {
-  this->ptzIface->Lock(1);
+  //this->ptzIface->Lock(1);
 
-  this->cmdPan = this->ptzIface->data->cmd_pan;
-  this->cmdTilt = this->ptzIface->data->cmd_tilt;
+  // pan tilt command set in void Ros_PTZ::PTZCommandReceived() rather than from Iface
+  //this->cmdPan = this->ptzIface->data->cmd_pan;
+  //this->cmdTilt = this->ptzIface->data->cmd_tilt;
   //this->cmdZoom = this->hfov / this->ptzIface->data->cmd_zoom;
 
-  this->ptzIface->Unlock();
+  //this->ptzIface->Unlock();
 
   // Apply joint limits to commanded pan/tilt angles
   if (this->cmdTilt > M_PI*0.3)
@@ -165,6 +205,8 @@
   float tilt = this->cmdTilt - this->tiltJoint->GetAngle();
   float pan = this->cmdPan - this->panJoint->GetAngle();
 
+  std::cout << "command received : " << this->cmdPan << ":" << this->cmdTilt;
+  std::cout << " state : " << this->panJoint->GetAngle() << ":" << this->tiltJoint->GetAngle() << std::endl;
   this->tiltJoint->SetParam( dParamVel, **(this->motionGainP) * tilt);
   this->tiltJoint->SetParam( dParamFMax, **(this->forceP) );
 
@@ -199,5 +241,16 @@
 
   // New data is available
   this->ptzIface->Post();
+
+  this->lock.lock();
+  // also put data into ros message
+  PTZStateMessage.pan.pos_valid =1;
+  PTZStateMessage.pan.pos       = this->panJoint->GetAngle();
+  PTZStateMessage.tilt.pos_valid=1;
+  PTZStateMessage.tilt.pos      = this->tiltJoint->GetAngle();
+  // publish topic
+  this->rosnode->publish(this->stateTopicName,PTZStateMessage);
+  this->lock.unlock();
+
 }
 

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-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml	2008-09-02 18:33:55 UTC (rev 3868)
@@ -30,19 +30,23 @@
   <verbatim key="controllers"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
 
   <!-- ptz camera controls -->
-  <controller:generic_ptz name="ptz_cam_left_controller">
+  <controller:Ros_PTZ name="ptz_cam_left_controller" plugin="libRos_PTZ.so">
     <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
     <panJoint>ptz_pan_left_joint</panJoint>
     <tiltJoint>ptz_tilt_left_joint</tiltJoint>
+    <commandTopicName>PTZL_cmd</commandTopicName>
+    <stateTopicName>PTZL_state</stateTopicName>
     <interface:ptz name="ptz_left_iface" />
-  </controller:generic_ptz>
+  </controller:Ros_PTZ>
 
-  <controller:generic_ptz name="ptz_cam_right_controller">
+  <controller:Ros_PTZ name="ptz_cam_right_controller" plugin="libRos_PTZ.so">
     <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
     <panJoint>ptz_pan_right_joint</panJoint>
     <tiltJoint>ptz_tilt_right_joint</tiltJoint>
+    <commandTopicName>PTZR_cmd</commandTopicName>
+    <stateTopicName>PTZR_state</stateTopicName>
     <interface:ptz name="ptz_right_iface" />
-  </controller:generic_ptz>
+  </controller:Ros_PTZ>
 
   <!-- P3D for position groundtruth -->
   <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test_headless.xml	2008-09-02 18:33:55 UTC (rev 3868)
@@ -30,19 +30,23 @@
   <verbatim key="controllers"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
 
   <!-- ptz camera controls -->
-  <controller:generic_ptz name="ptz_cam_left_controller">
+  <controller:Ros_PTZ name="ptz_cam_left_controller" plugin="libRos_PTZ.so">
     <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
     <panJoint>ptz_pan_left_joint</panJoint>
     <tiltJoint>ptz_tilt_left_joint</tiltJoint>
+    <commandTopicName>PTZL_cmd</commandTopicName>
+    <stateTopicName>PTZL_state</stateTopicName>
     <interface:ptz name="ptz_left_iface" />
-  </controller:generic_ptz>
+  </controller:Ros_PTZ>
 
-  <controller:generic_ptz name="ptz_cam_right_controller">
+  <controller:Ros_PTZ name="ptz_cam_right_controller" plugin="libRos_PTZ.so">
     <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
     <panJoint>ptz_pan_right_joint</panJoint>
     <tiltJoint>ptz_tilt_right_joint</tiltJoint>
+    <commandTopicName>PTZR_cmd</commandTopicName>
+    <stateTopicName>PTZR_state</stateTopicName>
     <interface:ptz name="ptz_right_iface" />
-  </controller:generic_ptz>
+  </controller:Ros_PTZ>
 
   <!-- P3D for position groundtruth -->
   <controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">

Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-09-02 18:29:06 UTC (rev 3867)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml	2008-09-02 18:33:55 UTC (rev 3868)
@@ -831,7 +831,7 @@
     <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 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+    <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate 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>
@@ -841,7 +841,7 @@
     <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 0 1 " /> <!-- direction of the joint in a local coordinate frame -->
+    <axis xyz=" 0 1 0 " /> <!-- direction of the joint in a local coordinate 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.