Personal Robots

Revision: 4587
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4587&view=rev
Author:   sachinchitta
Date:     2008-09-23 03:05:19 +0000 (Tue, 23 Sep 2008)

Log Message:
-----------
Tuned controller for the base. WARNING:: low level velocity controller tuning in Gazebo is still a little iffy, so the speed controllers for the wheels themselves are not very accurate because of which the base velocity control might not be accurate. 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-09-23 02:56:59 UTC (rev 4586)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-09-23 03:05:19 UTC (rev 4587)
@@ -319,6 +319,7 @@
 
     int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odometry message will be published on ROS */
 
+    double caster_steer_vel_gain_;
   };
 
   class BaseControllerNode : public Controller

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-23 02:56:59 UTC (rev 4586)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-23 03:05:19 UTC (rev 4587)
@@ -61,7 +61,7 @@
   base_odom_velocity_.z = 0;
   ils_weight_type_ = "Gaussian";
   ils_max_iterations_ = 3;
-
+  caster_steer_vel_gain_ = 0;
   pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
@@ -225,8 +225,11 @@
         if(elt_key->Attribute("key") == std::string("kp_speed"))
         {
           kp_speed_ = atof(elt_key->GetText());
-          break;
         }
+        if(elt_key->Attribute("key") == std::string("kp_caster_steer"))
+        {
+           caster_steer_vel_gain_ = atof(elt_key->GetText());
+        }
         elt_key = elt->NextSiblingElement("elem");
       }
     }
@@ -281,7 +284,7 @@
     cmd_vel_.z = cmd_vel_t_.z;
     pthread_mutex_unlock(&base_controller_lock_);
   }
-  //std::cout << "command received in update : " << cmd_vel_ << std::endl;
+//  std::cout << "command received in update : " << cmd_vel_ << std::endl;
 
   getJointValues();
 
@@ -344,7 +347,7 @@
     }
     steer_velocity_desired_[i] = -kp_speed_*error_steer;
 
-//    printf("Steering cmd: %d, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i]);
+//    printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
   }
 }
 
@@ -368,8 +371,7 @@
   double steer_angle_actual = 0;
   for(int i=0; i < (int) num_wheels_; i++)
   {
-
-    caster_2d_velocity.z = 0.01*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
+    caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
     steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
     wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
 
@@ -379,7 +381,7 @@
 //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
     wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
     wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-    //std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd_[i] << " r:" << wheel_radius_ << "caster: " << wheel_caster_steer_component.x << std::endl;
+//    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd_[i] << ", actual wheel speed" << wheel_speed_actual_[i] << ", caster: " << wheel_caster_steer_component.x/wheel_radius_ << ", caster_steer " << caster_2d_velocity.z << std::endl;
   }
 }
 
@@ -653,7 +655,7 @@
   base_odom_velocity_.y = (double)D.element(1,0);
   base_odom_velocity_.z = (double)D.element(2,0);
 
-  //cout << "Base odom velocity " << base_odom_velocity_ << endl;
+//  cout << "Base odom velocity " << base_odom_velocity_ << endl;
 }
 
 Matrix BaseController::pseudoInverse(const Matrix M)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 4606
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4606&view=rev
Author:   sachinchitta
Date:     2008-09-23 20:41:21 +0000 (Tue, 23 Sep 2008)

Log Message:
-----------
Modified base controller with changed gains should be more stable. 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-23 20:12:08 UTC (rev 4605)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-23 20:41:21 UTC (rev 4606)
@@ -347,7 +347,7 @@
     }
     steer_velocity_desired_[i] = -kp_speed_*error_steer;
 
-//    printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
+    //  printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
   }
 }
 
@@ -634,11 +634,21 @@
   Matrix C(2*num_wheels_,3);
   Matrix D(3,1);
   double steer_angle;
+  double wheel_speed;
+  libTF::Vector caster_2d_velocity;
+  libTF::Vector wheel_caster_steer_component;
 
+  caster_2d_velocity.x = 0;
+  caster_2d_velocity.y = 0;
+
   for(int i = 0; i < num_wheels_; i++) {
+    caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
+    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_; 
+
     steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*(wheel_speed_actual_[i]);
-    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*(wheel_speed_actual_[i]);
+    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
+    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
   }
 
   for(int i = 0; i < num_wheels_; i++) {

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-23 20:12:08 UTC (rev 4605)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-23 20:41:21 UTC (rev 4606)
@@ -19,6 +19,15 @@
   ros::init(argc,argv);
   ros::node *node = new ros::node("test_base_controller"); //Can be found by ros::node *ros::g_node for now.  Will eventually be superceded by ros::peer interface
 
+
+// Listen for base odom messages
+
+
+
+
+
+
+
   printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]);
   char *xml_robot_file = argv[1];
   TiXmlDocument xml(xml_robot_file);   // Load robot description


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 4658
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4658&view=rev
Author:   sachinchitta
Date:     2008-09-25 00:58:53 +0000 (Thu, 25 Sep 2008)

Log Message:
-----------
(a) test functions for basic base controller functions - xml loading, etc.

(b) test functions for running base controller - needs gazebo running to test out, prints out odometry speeds and ground_truth speeds

(c) base now has timeout on commands and will timeout (by default) in 3 seconds if no command is received on topic "cmd_vel"

(d) timeout can be changed using xml field in controllers.xml ("timeout")

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-25 00:28:14 UTC (rev 4657)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-25 00:58:53 UTC (rev 4658)
@@ -13,3 +13,6 @@
 
 rospack_add_executable(testBaseController test/test_base_controller.cpp)
 target_link_libraries(testBaseController pr2_mechanism_controllers)
+
+rospack_add_executable(testRunBaseController test/test_run_base_controller.cpp)
+target_link_libraries(testRunBaseController pr2_mechanism_controllers)

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-09-25 00:28:14 UTC (rev 4657)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-09-25 00:58:53 UTC (rev 4658)
@@ -320,6 +320,10 @@
     int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odometry message will be published on ROS */
 
     double caster_steer_vel_gain_;
+
+    double cmd_received_timestamp_;
+
+    double timeout_;
   };
 
   class BaseControllerNode : public Controller
@@ -350,10 +354,10 @@
 
     void setCommand(double vx, double vy, double vw);
 
+    BaseController *c_;
+
     private:
 
-    BaseController *c_;
-
     /*!
      * \brief deal with cmd_vel command from 2dnav stack
      */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-25 00:28:14 UTC (rev 4657)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-09-25 00:58:53 UTC (rev 4658)
@@ -62,6 +62,7 @@
   ils_weight_type_ = "Gaussian";
   ils_max_iterations_ = 3;
   caster_steer_vel_gain_ = 0;
+  timeout_ = 3;
   pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
@@ -73,10 +74,11 @@
 void BaseController::setCommand(libTF::Vector cmd_vel)
 {
   pthread_mutex_lock(&base_controller_lock_);
-  //std::cout << "command received : " << cmd_vel_t_ << std::endl;
   cmd_vel_t_.x = cmd_vel.x;
   cmd_vel_t_.y = cmd_vel.y;
   cmd_vel_t_.z = cmd_vel.z;
+  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+//  std::cout << "command received : " << cmd_vel_t_ << std::endl;
   pthread_mutex_unlock(&base_controller_lock_);
 }
 
@@ -183,6 +185,8 @@
   robot_state_ = robot_state;
 
   last_time_ = robot_state_->hw_->current_time_;
+
+  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 }
 
 bool BaseController::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
@@ -210,7 +214,6 @@
     std::cout << " sub controller : " << elt->Attribute("name") << std::endl;
 
     elt = elt->NextSiblingElement("controller");
-
   }
 
   elt = config->FirstChildElement("map");
@@ -219,7 +222,6 @@
     if(elt->Attribute("name") == std::string("velocity_control"))
     {
       TiXmlElement *elt_key = elt->FirstChildElement("elem");
-//      cout << "elt_key" << elt_key->Attribute("key") << endl;
       while(elt_key)
       {
         if(elt_key->Attribute("key") == std::string("kp_speed"))
@@ -230,12 +232,18 @@
         {
            caster_steer_vel_gain_ = atof(elt_key->GetText());
         }
-        elt_key = elt->NextSiblingElement("elem");
+        if(elt_key->Attribute("key") == std::string("timeout"))
+        {
+           timeout_ = atof(elt_key->GetText());
+        }
+        elt_key = elt_key->NextSiblingElement("elem");
       }
     }
+    std::cout << "*************************************" << std::endl;
 //    std::cout << " sub map : " << elt->Attribute("name") << std::endl;
     elt = config->NextSiblingElement("map");
   }
+
   init(jcp_vec,robot_state);
   return true;
 }
@@ -279,9 +287,20 @@
 
   if(pthread_mutex_trylock(&base_controller_lock_)==0)
   {
+    if((robot_state_->hw_->current_time_ - cmd_received_timestamp_) > timeout_)
+    {
+//      cout << "BaseController:: timing out" << endl;
+    cmd_vel_.x = 0;
+    cmd_vel_.y = 0;
+    cmd_vel_.z = 0;
+    }
+    else
+    {
+//      cout << "BaseController:: running" << endl;
     cmd_vel_.x = cmd_vel_t_.x;
     cmd_vel_.y = cmd_vel_t_.y;
     cmd_vel_.z = cmd_vel_t_.z;
+    }
     pthread_mutex_unlock(&base_controller_lock_);
   }
 //  std::cout << "command received in update : " << cmd_vel_ << std::endl;
@@ -345,7 +364,7 @@
       error_steer = error_steer_m_pi;
       steer_angle_desired = steer_angle_desired_m_pi;
     }
-    steer_velocity_desired_[i] = -kp_speed_*error_steer;
+    steer_velocity_desired_[i] =  -kp_speed_*error_steer;
 
     //  printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
   }
@@ -374,13 +393,14 @@
     caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
     steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
     wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
-
+//    cout << "gain " << caster_steer_vel_gain_ << endl;
 //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
 
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
 //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
     wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
     wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+
 //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd_[i] << ", actual wheel speed" << wheel_speed_actual_[i] << ", caster: " << wheel_caster_steer_component.x/wheel_radius_ << ", caster_steer " << caster_2d_velocity.z << std::endl;
   }
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-25 00:28:14 UTC (rev 4657)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-25 00:58:53 UTC (rev 4658)
@@ -3,52 +3,54 @@
 #include <mechanism_model/robot.h>
 #include <hardware_interface/hardware_interface.h>
 #include <ros/node.h>
+#include <urdf/parser.h>
 
+#include <libTF/libTF.h>
+
 int main( int argc, char** argv )
 {
-  mechanism::Robot *robot_model = new mechanism::Robot;
-  controller::BaseController bc;
-  HardwareInterface hw(0);
-
   if(argc != 3)
   {
     printf("Usage: %s <robot_xml> <controller_xml>\n",argv[0]);
     exit(-1);
   }
+  printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]);
 
-  ros::init(argc,argv);
-  ros::node *node = new ros::node("test_base_controller"); //Can be found by ros::node *ros::g_node for now.  Will eventually be superceded by ros::peer interface
 
+  /*********** Create the robot model ****************/
+  mechanism::Robot *robot_model = new mechanism::Robot;
+  controller::BaseControllerNode bc;
+  HardwareInterface hw(0);
+  robot_model->hw_ = &hw;
 
-// Listen for base odom messages
 
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("test_base_controller"); 
 
 
-
-
-
-
-  printf("robot file:: %s, controller file:: %s\n",argv[1],argv[2]);
+  /*********** Load the robot model and state file ************/
   char *xml_robot_file = argv[1];
   TiXmlDocument xml(xml_robot_file);   // Load robot description
   xml.LoadFile();
   TiXmlElement *root = xml.FirstChildElement("robot");
-
+  urdf::normalizeXml(root);
   robot_model->initXml(root);
+  mechanism::RobotState *robot_state = new mechanism::RobotState(robot_model, &hw);
 
 
+  /*********** Load the controller file ************/
   char *xml_control_file = argv[2];
   TiXmlDocument xml_control(xml_control_file);   // Load robot description
   xml_control.LoadFile();
-  TiXmlElement *root_control = xml_control.FirstChildElement("robot");
-  TiXmlElement *root_controller = root_control->FirstChildElement("controller");
+  TiXmlElement *root_control = xml_control.FirstChildElement("controllers");
+  TiXmlElement *root_controller = root_control->FirstChildElement("controller");  
+  bc.initXml(robot_state,root_controller);
 
-  mechanism::RobotState *robot_state = new mechanism::RobotState(robot_model, &hw);
 
-  bc.initXml(robot_state,root_controller);
+  /************ Testing the odometry calculations themselves ******************/
+/*  NEWMAT::Matrix A(16,3);
 
-  NEWMAT::Matrix A(16,3);
-
   A.Row(1) << 10 << 8.95 << 0.05;
   A.Row(2) << 0 <<  -2 << 0;
   A.Row(3) << 1 << -0.1 << 0.01;
@@ -71,19 +73,21 @@
 
   NEWMAT::Matrix B(16,1);
   B << 1.1 << 1 << 1.1 << 1.15 << 0.95 << 0.99 << 0.98 << 0.95 << 1.05 << 1.1 << 1.05 << 1 << 1.13 << 0.995 << 1.035 << 1.08;
-
   NEWMAT::Matrix xfit(3,1);
 
-  xfit = bc.iterativeLeastSquares(A,B,"Gaussian",10);
-
+  xfit = bc.c_->iterativeLeastSquares(A,B,"Gaussian",10);
   cout << "done" << xfit << endl;
+*/  
   ros::fini();
   delete robot_model;
   delete robot_state;
-//void BaseController::initXml(mechanism::Robot *robot, TiXmlElement *config)
 }
 
 
+
+
+
+
 /*class BaseControllerTest : public testing::Test {
  protected:
   virtual void SetUp() {

Added: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-09-25 00:58:53 UTC (rev 4658)
@@ -0,0 +1,125 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <libTF/libTF.h>
+#include <ros/node.h>
+#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/BaseVel.h>
+#include <std_msgs/RobotBase2DOdom.h>
+
+static int done = 0;
+
+void finalize(int donecare)
+{
+  done = 1;
+}
+
+class test_run_base
+{
+  public:
+
+    test_run_base():subscriber_connected(0){}; 
+
+    ~test_run_base() {}
+
+    std_msgs::TransformWithRateStamped ground_truth;
+
+    std_msgs::RobotBase2DOdom odom;
+
+    int subscriber_connected;
+
+    void subscriber_connect()
+    {
+      subscriber_connected = 1;
+    }
+
+    void odomMsgReceived()
+    {
+//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
+    };
+
+    void groundTruthMsgReceived()
+    {
+//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
+    };
+};
+
+int main( int argc, char** argv )
+{
+
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("test_run_base_controller"); 
+
+
+  // receive messages from 2dnav stack
+  std_msgs::TransformWithRateStamped ground_truth;
+
+  test_run_base tb;
+
+  node->subscribe("base_pose_ground_truth",tb.ground_truth,&test_run_base::odomMsgReceived,&tb,10);
+
+  node->subscribe("odom",tb.odom,&test_run_base::groundTruthMsgReceived,&tb,10);
+
+  signal(SIGINT,  finalize);
+  signal(SIGQUIT, finalize);
+  signal(SIGTERM, finalize);
+
+
+  /*********** Start moving the robot ************/
+  std_msgs::BaseVel cmd;
+  cmd.vx = -1.0;
+  cmd.vy = 0;
+  cmd.vw = 0;
+
+  node->advertise<std_msgs::BaseVel>("cmd_vel",10);
+  sleep(1);
+  node->publish("cmd_vel",cmd);
+  sleep(1);
+
+  double start_x = tb.ground_truth.transform.translation.x;
+  double start_y = tb.ground_truth.transform.translation.y;
+  double start_th = tb.ground_truth.transform.translation.z;
+
+  while(!done)
+  {
+    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " " << tb.ground_truth.rate.translation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+
+    cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+
+    node->publish("cmd_vel",cmd);
+
+    sleep(1);
+  }
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 4741
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4741&view=rev
Author:   sachinchitta
Date:     2008-09-27 00:58:09 +0000 (Sat, 27 Sep 2008)

Log Message:
-----------
Adding inverse dynamics controller (untested, don't use yet)

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-27 00:57:16 UTC (rev 4740)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-27 00:58:09 UTC (rev 4741)
@@ -9,10 +9,14 @@
   src/base_controller.cpp
   src/laser_scanner_controller.cpp
   src/arm_position_controller.cpp
-  src/arm_velocity_controller.cpp)
+  src/arm_velocity_controller.cpp
+  src/arm_dynamics_controller.cpp)
 
 rospack_add_executable(testBaseController test/test_base_controller.cpp)
 target_link_libraries(testBaseController pr2_mechanism_controllers)
 
 rospack_add_executable(testRunBaseController test/test_run_base_controller.cpp)
 target_link_libraries(testRunBaseController pr2_mechanism_controllers)
+
+rospack_add_executable(testArmDynamicsController test/test_arm_dynamics_controller.cpp)
+target_link_libraries(testRunBaseController pr2_mechanism_controllers)

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,226 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+
+// Services
+#include <pr2_mechanism_controllers/SetJointCmd.h>
+#include <pr2_mechanism_controllers/GetJointCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointGains.h>
+#include <pr2_mechanism_controllers/GetJointGains.h>
+
+//Kinematics
+#include <robot_kinematics/robot_kinematics.h>
+
+// #include <libTF/Pose3D.h>
+// #include <urdf/URDF.h>
+
+namespace controller
+{
+
+// The maximum number of joints expected in an arm.
+static const int MAX_ARM_JOINTS = 7;
+
+class ArmDynamicsController : public Controller
+{
+public:
+
+  /*!
+   * \brief Default Constructor of the JointController class.
+   *
+   */
+  ArmDynamicsController();
+
+  /*!
+   * \brief Destructor of the JointController class.
+   */
+  virtual ~ArmDynamicsController();
+
+  /*!
+   * \brief Functional way to initialize limits and gains.
+   *
+  */
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+  /*!
+   * \brief Sets a goal for some joint, specified as a pair of (joint values, joint names)
+   *
+   * \param j_values the positions of the joints
+   * \param j_names the names of the joints
+  */
+  void setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot, const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names);
+
+//  /**
+//   * @brief Overloaded method for convenience
+//   * @param req
+//   */
+//  void setJointCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
+
+  void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+
+  /*!
+   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+  */
+//  void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+
+//  void getCurrentConfiguration(std::vector<double> &);
+
+  /*!
+     * \brief Issues commands to the joint. Should be called at regular intervals
+  */
+  virtual void update(void); // Real time safe.
+
+  ros::thread::mutex arm_controller_lock_;
+
+//  void setJointGains(const pr2_mechanism_controllers::SetJointGains::request &req);
+
+//  void getJointGains(pr2_mechanism_controllers::GetJointGains::response &resp);
+
+  controller::JointEffortController* getJointEffortControllerByName(std::string name);
+
+  bool goalAchieved() const { return goal_achieved_; }
+
+private:
+
+  std::vector<JointEffortController *> joint_effort_controllers_;
+
+  // Goal of the joints
+  std::vector<double> goals_;
+
+  // Goal of the joints
+  std::vector<double> goals_dot_;
+
+  // Goal of the joints
+  std::vector<double> goals_dot_dot_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_dot_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_dot_dot_;
+
+  mechanism::Robot* robot_;
+
+  void updateJointControllers(void);
+
+  int getJointControllerByName(std::string name);
+
+  void checkForGoalAchieved_(void);
+
+  bool goal_achieved_;
+
+  KDL::Vector *kdl_torque_;
+
+  robot_kinematics::SerialChain *arm_chain_;
+
+  // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
+  bool refresh_rt_vals_;
+
+};
+
+/** @class ArmDynamicsControllerNode
+ *  @brief ROS interface for the arm controller.
+ *  @author Timothy Hunter <tjhunter@...>
+ *
+ *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
+ *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
+ *
+ */
+class ArmDynamicsControllerNode : public Controller
+{
+  public:
+    /*!
+    * \brief Default Constructor
+    *
+    */
+    ArmDynamicsControllerNode();
+
+    /*!
+    * \brief Destructor
+    */
+    ~ArmDynamicsControllerNode();
+
+    void update();
+
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /** @brief sets a command for all the joints managed by the controller at once
+     * This is a lightweight version of setJointPosHeadless when the caller knows the order of the joints.
+     * @note this service should not be used. use setJointPosHeadless instead
+     * This service returns immediately
+     * @param req an array of position
+     * @param resp The response is empty
+     * @return
+     */
+    bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+                    pr2_mechanism_controllers::SetJointCmd::response &resp);
+
+    /** @brief service that returns the goal of the controller
+     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
+     * @param req
+     * @param resp the response, contains a JointPosCmd message with the goal of the controller
+     * @return
+     */
+    bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
+                        pr2_mechanism_controllers::GetJointCmd::response &resp);
+
+  private:
+    pr2_mechanism_controllers::JointCmd msg_;   //The message used by the ROS callback
+    ArmDynamicsController *c_;
+
+    robot_kinematics::RobotKinematics pr2_kin_;
+
+    robot_kinematics::SerialChain * arm_chain_;
+};
+
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointCmd.msg	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,4 @@
+string[] names
+float64[] positions
+float64[] velocity
+float64[] acc

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,278 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Sachin Chitta <sachinc@...>
+
+#include "pr2_mechanism_controllers/arm_dynamics_controller.h"
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(ArmDynamicsController);
+
+ArmDynamicsController::ArmDynamicsController()
+{
+}
+
+ArmDynamicsController::~ArmDynamicsController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i<joint_effort_controllers_.size();++i)
+    delete joint_effort_controllers_[i];
+
+  delete kdl_torque_;
+}
+
+bool ArmDynamicsController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  robot_ = robot->model_;
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    JointEffortController * jpc = new JointEffortController();
+    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointEffortController"));
+    joint_effort_controllers_.push_back(jpc);
+    if(!jpc->initXml(robot, elt))
+      return false;
+
+    elt = elt->NextSiblingElement("controller");
+  }
+  goals_.resize(joint_effort_controllers_.size());
+  goals_rt_.resize(joint_effort_controllers_.size());
+
+  kdl_torque_ = new KDL::Vector[joint_effort_controllers_.size()+1];
+
+  return true;
+}
+
+void ArmDynamicsController::setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot,  const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names)
+{
+  assert(j_values.size() == j_names.size());
+  for(uint i = 0; i < j_values.size(); ++i)
+  {
+    const std::string & name = j_names[i];
+    const int id = getJointControllerByName(name);
+    if(id >= 0)
+    {
+      goals_[id] = j_values[i];
+      goals_dot_[id] = j_values_dot[i];
+      goals_dot_dot_[id] = j_values_dot_dot[i];
+    }
+  }
+  goal_achieved_ = false;
+
+  arm_controller_lock_.lock();
+  refresh_rt_vals_ = true;
+  arm_controller_lock_.unlock();
+}
+
+
+void ArmDynamicsController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+{
+  const unsigned int n = joint_effort_controllers_.size();
+  cmd.set_names_size(n);
+  for(unsigned int i=0; i<n; ++i)
+      cmd.names[i] = joint_effort_controllers_[i]->getJointName();
+
+  cmd.set_positions_vec(goals_);
+}
+
+
+controller::JointEffortController* ArmDynamicsController::getJointEffortControllerByName(std::string name)
+{
+  for(int i=0; i< (int) joint_effort_controllers_.size(); i++)
+  {
+    if(joint_effort_controllers_[i]->getJointName() == name)
+    {
+      return joint_effort_controllers_[i];
+    }
+  }
+    return NULL;
+}
+
+
+int ArmDynamicsController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) joint_effort_controllers_.size(); i++)
+  {
+    if(joint_effort_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+
+void ArmDynamicsController::update(void)
+{
+   cout << "Updating dynamics controller " << std::endl;
+   KDL::JntArray kdl_q(goals_rt_.size());
+   KDL::JntArray kdl_q_dot(goals_rt_.size());
+   KDL::JntArray kdl_q_dot_dot(goals_rt_.size());
+
+  if(refresh_rt_vals_ && arm_controller_lock_.trylock())
+  {
+    assert(goals_.size() == goals_rt_.size());
+    for(unsigned int i=0; i<goals_.size(); ++i)
+    {
+      goals_rt_[i] = goals_[i];
+      goals_rt_dot_[i] = goals_dot_[i];
+      goals_rt_dot_dot_[i] = goals_dot_dot_[i];
+    }
+    arm_controller_lock_.unlock();
+  }
+
+    for(unsigned int i=0; i<goals_.size(); ++i)
+    {
+       kdl_q(i) = joint_effort_controllers_[i]->joint_->position_;
+       kdl_q_dot(i) = 0.0;
+       kdl_q_dot_dot(i) = 0.0;
+    }
+
+    arm_chain_->computeInverseDynamics(kdl_q,kdl_q_dot,kdl_q_dot_dot,kdl_torque_);
+
+  for(unsigned int i=0;i<goals_rt_.size();++i)
+  {
+     //   joint_effort_controllers_[i]->setCommand(kdl_torque_[i][2]);
+   joint_effort_controllers_[i]->setCommand(0.0);
+//     fprintf(stderr,"Effort: %d %f\n",i,kdl_torque_[i][2]);
+  }
+  updateJointControllers();
+}
+
+void ArmDynamicsController::updateJointControllers(void)
+{
+  for(unsigned int i=0;i<goals_rt_.size();++i)
+    joint_effort_controllers_[i]->update();
+}
+
+
+void ArmDynamicsController::checkForGoalAchieved_(void)
+{
+/*  goal_achieved_ = true;
+  for(unsigned int i=0;i<goals_rt_.size();++i)
+    goal_achieved_=goal_achieved_&&(error_margins_[i]<=0||std::abs(joint_effort_controllers_[i]->getMeasuredPosition()-goals_rt_[i])<error_margins_[i]);
+*/
+}
+
+
+//------ Arm controller node --------
+
+ROS_REGISTER_CONTROLLER(ArmDynamicsControllerNode)
+
+ArmDynamicsControllerNode::ArmDynamicsControllerNode()
+  : Controller()
+{
+  std::cout<<"Controller node created"<<endl;
+  c_ = new ArmDynamicsController();
+}
+
+ArmDynamicsControllerNode::~ArmDynamicsControllerNode()
+{
+  delete c_;
+}
+
+void ArmDynamicsControllerNode::update()
+{
+  c_->update();
+}
+
+bool ArmDynamicsControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  std::cout<<"LOADING ARM DYNAMICS CONTROLLER NODE"<<std::endl;
+  ros::node * const node = ros::node::instance();
+  string prefix = config->Attribute("name");
+  std::cout<<"the prefix is "<<prefix<<std::endl;
+
+  // Parses controller configuration.
+  std::string kdl_chain_name="";
+  TiXmlElement *j = config->FirstChildElement("kinematics");
+  assert(j);
+  j = j->FirstChildElement("elem");
+  assert(j);
+  while(j!=NULL)
+  {
+    if(j->Attribute("key")==std::string("kdl_chain_name"))
+    {
+      kdl_chain_name = j->GetText();
+      break;
+    }
+    j = j->NextSiblingElement("elem");
+  }
+
+  // Parses kinematics description
+  std::string pr2Contents;
+  node->get_param("robotdesc/pr2", pr2Contents);
+  pr2_kin_.loadString(pr2Contents.c_str());
+  arm_chain_ = pr2_kin_.getSerialChain(kdl_chain_name.c_str());
+  fprintf(stderr,"Got arm chain %s\n",kdl_chain_name.c_str());
+  assert(arm_chain_);
+
+  // Parses subcontroller configuration
+  if(c_->initXml(robot, config))
+  {
+    node->advertise_service(prefix + "/set_command_array", &ArmDynamicsControllerNode::setJointSrv, this);
+    node->advertise_service(prefix + "/get_command", &ArmDynamicsControllerNode::getJointCmd, this);
+    return true;
+  }
+  return false;
+}
+
+bool ArmDynamicsControllerNode::setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+                                   pr2_mechanism_controllers::SetJointCmd::response &resp)
+{
+  std::vector<double> pos;
+  std::vector<double> vel;
+  std::vector<double> acc;
+  std::vector<std::string> names;
+
+  req.set_positions_vec(pos);
+  req.set_velocity_vec(vel);
+  req.set_acc_vec(acc);
+  req.set_names_vec(names);
+
+  c_->setJointCmd(pos,vel,acc,names);
+  return true;
+}
+
+bool ArmDynamicsControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
+                    pr2_mechanism_controllers::GetJointCmd::response &resp)
+{
+  pr2_mechanism_controllers::JointCmd cmd;
+  c_->getJointCmd(cmd);
+  resp.command = cmd;
+  return true;
+}

Added: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetJointCmd.srv	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,2 @@
+---
+JointCmd command
\ No newline at end of file

Added: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetJointCmd.srv	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,9 @@
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
+---
+float64[] positions
+float64[] velocity
+float64[] acc
+string[] names
\ No newline at end of file

Added: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	2008-09-27 00:58:09 UTC (rev 4741)
@@ -0,0 +1,105 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <libTF/libTF.h>
+#include <ros/node.h>
+#include <pr2_mechanism_controllers/SetJointCmd.h>
+#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/BaseVel.h>
+#include <std_msgs/RobotBase2DOdom.h>
+
+static int done = 0;
+
+void finalize(int donecare)
+{
+  done = 1;
+}
+
+/* class test_run_arm
+{
+   public:
+
+      test_run_arm(){}; 
+
+      ~test_run_arm(){};
+
+      mechanism_control::MechanismState state;
+
+      void jointMsgReceived()
+      {
+
+      };
+};
+*/
+
+
+int main( int argc, char** argv )
+{
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("test_arm_dynamics_controller"); 
+
+  signal(SIGINT,  finalize);
+  signal(SIGQUIT, finalize);
+  signal(SIGTERM, finalize);
+
+  /*********** Start moving the arm ************/
+  pr2_mechanism_controllers::SetJointCmd::request  req;
+  pr2_mechanism_controllers::SetJointCmd::response resp;
+
+  int num_joints = 7;
+
+  req.set_positions_size(num_joints);
+  req.set_velocity_size(num_joints);
+  req.set_acc_size(num_joints);
+
+  resp.set_positions_size(num_joints);
+  resp.set_velocity_size(num_joints);
+  resp.set_acc_size(num_joints);
+
+  for(int i=0; i < num_joints; i++)
+  {
+     req.positions[i] = 0.0;
+     req.velocity[i]  = 0.0;
+     req.acc[i]       = 0.0;
+  }
+
+  sleep(1);
+
+//  node->subscribe("mechanism_state",tra.state,&test_run_arm::jointMsgReceived,&tra,10);
+//  if(ros::service::call("arm_dynamics_controller/set_command_array",req,resp))
+//  {
+//  }
+
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-27 00:57:16 UTC (rev 4740)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-09-27 00:58:09 UTC (rev 4741)
@@ -1,9 +1,9 @@
 
 #include <pr2_mechanism_controllers/base_controller.h>
+#include <hardware_interface/hardware_interface.h>
 #include <mechanism_model/robot.h>
-#include <hardware_interface/hardware_interface.h>
+#include <urdf/parser.h>
 #include <ros/node.h>
-#include <urdf/parser.h>
 
 #include <libTF/libTF.h>
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 4833
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4833&view=rev
Author:   vijaypradeep
Date:     2008-09-30 20:48:22 +0000 (Tue, 30 Sep 2008)

Log Message:
-----------
Fixed DavidLi's laserscanner velocity controller to match the new controller directory scheme

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp

Removed Paths:
-------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller_david.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller_david.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-30 20:45:48 UTC (rev 4832)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-09-30 20:48:22 UTC (rev 4833)
@@ -8,6 +8,7 @@
 rospack_add_library( pr2_mechanism_controllers
   src/base_controller.cpp
   src/laser_scanner_controller.cpp
+  src/laser_scanner_velocity_controller.cpp
   src/arm_position_controller.cpp
   src/arm_velocity_controller.cpp
   src/arm_dynamics_controller.cpp)

Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller_david.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller_david.h	2008-09-30 20:45:48 UTC (rev 4832)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller_david.h	2008-09-30 20:48:22 UTC (rev 4833)
@@ -1,216 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2008, Willow Garage, Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Willow Garage nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#pragma once
-
-#include <ros/node.h>
-
-#include <mechanism_model/controller.h>
-#include <robot_mechanism_controllers/joint_position_controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-
-// Services
-#include <robot_mechanism_controllers/SetCommand.h>
-#include <robot_mechanism_controllers/GetCommand.h>
-#include <robot_mechanism_controllers/SetProfile.h>
-#include <robot_mechanism_controllers/SetPosition.h>
-#include <robot_mechanism_controllers/GetPosition.h>
-namespace controller
-{
-
-class LaserScannerController : public Controller
-{
-
-public:
-  //Indicates whether we close the loop around position or velocity
-  enum LaserControllerMode
-  {
-    VELOCITY,POSITION
-  };
-
-  /*!
-   * \brief Default Constructor of the JointController class.
-   *
-   */
-  LaserScannerController();
-
-  /*!
-   * \brief Destructor of the JointController class.
-   */
-  ~LaserScannerController();
-
-  /*!
-   * \brief Functional way to initialize limits and gains.
-   *
-   */
-  void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-  /*!
-   * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
-   *
-   * \param double pos Position command to issue
-   */
-  void setCommand(double command);
-
-  /*!
-   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
-   */
-  double getCommand();
-
-  /*!
-   * \brief Read the torque of the motor
-   */
-  double getMeasuredPosition();
-
-  /*!
-   * \brief Issues commands to the joint. Should be called at regular intervals
-   */
-
-  virtual void update();
-
-  /*!
-   * \brief Returns the time
-   */
-  double getTime();
-
-  double upper_turnaround_offset_; /*!<Distance from positive endstop where turnaround occurs>*/
-  double lower_turnaround_offset_;/*!<Distance from negative endstop where turnaround occurs>*/
-
-  double upper_deceleration_buffer_;/*!<Distance from positive turnaround where deceleration occurs>*/
-  double lower_deceleration_buffer_;/*!<Distance from negative turnaround where deceleration occurs>*/
-
-  bool passed_center_; /*!<Marker that indicates that we've recently moved past the center point>*/
-  double last_position_; /*!<Record last read position>*/
-  bool automatic_turnaround_; /*!<Do we automatically turn around at edges of workspace?>*/
-
-  void setTurnaroundPoints(void);
-
-  LaserControllerMode current_mode_; /*!<Indicates the current status of the controller>*/
-
-  private:
-
-  double upper_deceleration_zone_; /*!<Location near upper endstop where deceleration starts>*/
-  double upper_turnaround_location_;/*!<Location near upper endstop where turnaround actually occurs>*/
-
-  double lower_deceleration_zone_;/*!<Location near lower endstop where deceleration starts>*/
-  double lower_turnaround_location_;/*!<Location near lower endstop where turnaround actually occurs>*/
-
-   /*!
-   * \brief Actually issue torque set command of the joint motor.
-   */
-  void setJointEffort(double torque);
-
-  mechanism::JointState* joint_; /*!< Joint we're controlling>*/
-  JointPositionController joint_position_controller_; /*!< Internal PID controller for position>*/
-  JointVelocityController joint_velocity_controller_;/*!< Internal PID controller for velocity>*/
-  double last_time_; /*!< Last time stamp of update> */
-  double command_; /*!< Last commanded position> */
-  mechanism::RobotState *robot_; /*!< Pointer to robot structure>*/
-
- };
-
-class LaserScannerControllerNode : public Controller
-{
-public:
-  /*!
-   * \brief Default Constructor
-   *
-   */
-  LaserScannerControllerNode();
-
-  /*!
-   * \brief Destructor
-   */
-  ~LaserScannerControllerNode();
-
-  double getMeasuredPosition();
-
-  void update();
-
-  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
-  // Services
-   /*!
-   * \brief Send velocity command
-   */
-
-  bool setCommand(robot_mechanism_controllers::SetCommand::request &req,
-                  robot_mechanism_controllers::SetCommand::response &resp);
- /*!
-   * \brief Send velocity command
-   */
-
-  bool getCommand(robot_mechanism_controllers::GetCommand::request &req,
-                  robot_mechanism_controllers::GetCommand::response &resp);
- /*!
-   * \brief Send velocity command
-   */
-
-  bool setPosition(robot_mechanism_controllers::SetPosition::request &req,
-                  robot_mechanism_controllers::SetPosition::response &resp);
- /*!
-   * \brief Send velocity command
-   */
-
-  bool getPosition(robot_mechanism_controllers::GetPosition::request &req,
-                  robot_mechanism_controllers::GetPosition::response &resp);
- /*!
-   * \brief Send velocity command
-   */
-
-  bool getActual(robot_mechanism_controllers::GetActual::request &req,
-                  robot_mechanism_controllers::GetActual::response &resp);
-  /*!
-   * \brief Send velocity command
-   */
-
-  bool setProfile(robot_mechanism_controllers::SetProfile::request &req,
-  robot_mechanism_controllers::SetProfile::response &resp);
-  /*!
-   * \brief Send velocity command
-   */
-
-  bool setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer);
-
-
-  void setCommand(double command);
-  double getCommand();
-
-private:
-  LaserScannerController *c_;
-};
-}
-
-

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h (from rev 4814, pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller_david.h)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_velocity_controller.h	2008-09-30 20:48:22 UTC (rev 4833)
@@ -0,0 +1,216 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+
+// Services
+#include <robot_mechanism_controllers/SetCommand.h>
+#include <robot_mechanism_controllers/GetCommand.h>
+#include <pr2_mechanism_controllers/SetProfile.h>
+#include <robot_mechanism_controllers/SetPosition.h>
+#include <robot_mechanism_controllers/GetPosition.h>
+namespace controller
+{
+
+class LaserScannerVelocityController : public Controller
+{
+
+public:
+  //Indicates whether we close the loop around position or velocity
+  enum LaserControllerMode
+  {
+    VELOCITY,POSITION
+  };
+
+  /*!
+   * \brief Default Constructor of the JointController class.
+   *
+   */
+  LaserScannerVelocityController();
+
+  /*!
+   * \brief Destructor of the JointController class.
+   */
+  ~LaserScannerVelocityController();
+
+  /*!
+   * \brief Functional way to initialize limits and gains.
+   *
+   */
+  void init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot);
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  /*!
+   * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+   *
+   * \param double pos Position command to issue
+   */
+  void setCommand(double command);
+
+  /*!
+   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+   */
+  double getCommand();
+
+  /*!
+   * \brief Read the torque of the motor
+   */
+  double getMeasuredPosition();
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+
+  virtual void update();
+
+  /*!
+   * \brief Returns the time
+   */
+  double getTime();
+
+  double upper_turnaround_offset_; /*!<Distance from positive endstop where turnaround occurs>*/
+  double lower_turnaround_offset_;/*!<Distance from negative endstop where turnaround occurs>*/
+
+  double upper_deceleration_buffer_;/*!<Distance from positive turnaround where deceleration occurs>*/
+  double lower_deceleration_buffer_;/*!<Distance from negative turnaround where deceleration occurs>*/
+
+  bool passed_center_; /*!<Marker that indicates that we've recently moved past the center point>*/
+  double last_position_; /*!<Record last read position>*/
+  bool automatic_turnaround_; /*!<Do we automatically turn around at edges of workspace?>*/
+
+  void setTurnaroundPoints(void);
+
+  LaserControllerMode current_mode_; /*!<Indicates the current status of the controller>*/
+
+  private:
+
+  double upper_deceleration_zone_; /*!<Location near upper endstop where deceleration starts>*/
+  double upper_turnaround_location_;/*!<Location near upper endstop where turnaround actually occurs>*/
+
+  double lower_deceleration_zone_;/*!<Location near lower endstop where deceleration starts>*/
+  double lower_turnaround_location_;/*!<Location near lower endstop where turnaround actually occurs>*/
+
+   /*!
+   * \brief Actually issue torque set command of the joint motor.
+   */
+  void setJointEffort(double torque);
+
+  mechanism::JointState* joint_; /*!< Joint we're controlling>*/
+  JointPositionController joint_position_controller_; /*!< Internal PID controller for position>*/
+  JointVelocityController joint_velocity_controller_;/*!< Internal PID controller for velocity>*/
+  double last_time_; /*!< Last time stamp of update> */
+  double command_; /*!< Last commanded position> */
+  mechanism::RobotState *robot_; /*!< Pointer to robot structure>*/
+
+ };
+
+class LaserScannerVelocityControllerNode : public Controller
+{
+public:
+  /*!
+   * \brief Default Constructor
+   *
+   */
+  LaserScannerVelocityControllerNode();
+
+  /*!
+   * \brief Destructor
+   */
+  ~LaserScannerVelocityControllerNode();
+
+  double getMeasuredPosition();
+
+  void update();
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  // Services
+   /*!
+   * \brief Send velocity command
+   */
+
+  bool setCommand(robot_mechanism_controllers::SetCommand::request &req,
+                  robot_mechanism_controllers::SetCommand::response &resp);
+ /*!
+   * \brief Send velocity command
+   */
+
+  bool getCommand(robot_mechanism_controllers::GetCommand::request &req,
+                  robot_mechanism_controllers::GetCommand::response &resp);
+ /*!
+   * \brief Send velocity command
+   */
+
+  bool setPosition(robot_mechanism_controllers::SetPosition::request &req,
+                   robot_mechanism_controllers::SetPosition::response &resp);
+ /*!
+   * \brief Send velocity command
+   */
+
+  bool getPosition(robot_mechanism_controllers::GetPosition::request &req,
+                   robot_mechanism_controllers::GetPosition::response &resp);
+ /*!
+   * \brief Send velocity command
+   */
+
+  bool getActual(robot_mechanism_controllers::GetActual::request &req,
+                 robot_mechanism_controllers::GetActual::response &resp);
+  /*!
+   * \brief Send velocity command
+   */
+
+  bool setProfile(pr2_mechanism_controllers::SetProfile::request &req,
+                  pr2_mechanism_controllers::SetProfile::response &resp);
+  /*!
+   * \brief Send velocity command
+   */
+
+  bool setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer);
+
+
+  void setCommand(double command);
+  double getCommand();
+
+private:
+  LaserScannerVelocityController *c_;
+};
+}
+
+

Deleted: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller_david.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller_david.cpp	2008-09-30 20:45:48 UTC (rev 4832)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller_david.cpp	2008-09-30 20:48:22 UTC (rev 4833)
@@ -1,377 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2008, Willow Garage, Inc.
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of the Willow Garage nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#include <algorithm>
-
-#include <pr2_mechanism_controllers/laser_scanner_controller.h>
-#include <math_utils/angles.h>
-#include <math_utils/velocity.h>
-using namespace std;
-using namespace controller;
-#define MIN_MOVING_SPEED 0.3
-ROS_REGISTER_CONTROLLER(LaserScannerController)
-
-LaserScannerController::LaserScannerController()
-{
-  robot_ = NULL;
-  joint_ = NULL;
-
-  command_ = 0;
-  last_time_ = 0;
-  current_mode_ = VELOCITY; //Start out in velocity mode
-
-
-  passed_center_ = true;
-  upper_turnaround_offset_ = 0.0;
-  lower_turnaround_offset_ = 0.0;
-  automatic_turnaround_ = false;
-  upper_deceleration_buffer_ = 0.0;
-  lower_deceleration_buffer_ = 0.0;
-}
-
-LaserScannerController::~LaserScannerController()
-{
-
-}
-
-void LaserScannerController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
-{
-  robot_ = robot;
-  joint_ = robot->getJointState(name);
-
-  abort(); //joint_position_controller_.init( p_gain,  i_gain,  d_gain,  windup, time, name, robot);
-  command_= 0;
-  last_time_= time;
-}
-
-bool LaserScannerController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
-  assert(robot);
-
-
-  robot_ = robot;
-  last_time_ = robot->hw_->current_time_;
-
-  TiXmlElement *vel = config->FirstChildElement("velocity");
-
-  //Perform checks at highest level to give the most informative error message possible
-  TiXmlElement *j = vel->FirstChildElement("joint");
-  if (!j)
-  {
-    fprintf(stderr, "LaserScannerController was not given a joint\n");
-    return false;
-  }
-
-  const char *joint_name = j->Attribute("name");
-  int index = joint_name ? robot->model_->getJointIndex(joint_name) : -1;
-
-  if (index < 0)
-  {
-    fprintf(stderr, "JointPositionController could not find joint named \"%s\"\n", joint_name);
-    return false;
-  }
-  joint_ = &robot->joint_states_[index];
-
- TiXmlElement *p = j->FirstChildElement("pid");
-  if (!p)
-    fprintf(stderr, "LaserScannerController's config did not specify the default pid parameters.\n");
-
-  joint_velocity_controller_.initXml(robot,vel); //Pass down XML snippet to encapsulated joint_velocity_controller_
-
-  TiXmlElement *pos = config->FirstChildElement("position");
-
-  joint_position_controller_.initXml(robot,pos); //Pass down XML snippet to encapsulated joint_position_controller_
-
-  last_position_ = joint_->position_;
-  return true;
- }
-
-// Set the joint velocity command
-void LaserScannerController::setCommand(double command)
-{
-  command_ = command;
-  if(current_mode_ == VELOCITY && automatic_turnaround_)
-  {
-    if(joint_->position_> upper_turnaround_location_)
-    {
-      if(command_ > 0) command_ = -command_; //Turn command around to avoid crash
-      passed_center_ = false; //Allow passing through
-    }
-    else if (joint_->position_ < lower_turnaround_location_)
-    {
-      if(command_<0) command_ = -command_;
-      passed_center_ = false;
-    }
-  }
-}
-
-// Return the current position command
-double LaserScannerController::getCommand()
-{
-  return command_;
-}
-
-// Return the measured joint position
-double LaserScannerController::getMeasuredPosition()
-{
-  return joint_->position_;
-}
-
-double LaserScannerController::getTime()
-{
-  return robot_->hw_->current_time_;
-}
-
-void LaserScannerController::update()
-{
-  double time = robot_->hw_->current_time_;
-  double position = joint_->position_;
-  double truncated_command, ratio;
-
-  switch(current_mode_)
-  {
-    case VELOCITY:
-
-     joint_velocity_controller_.setCommand(command_);
-
-    if(automatic_turnaround_) //Automatically turn around
-    {
-       //Track if we passed center point
-      if((position>=0 && last_position_<=0) || (position<=0 && last_position_>=0))  passed_center_ = true;
-
-      if(passed_center_) //Check whether we're moving towards the edge
-      {
-
-        if(position >= upper_deceleration_zone_)
-        {
-          //Scale command for deceleration
-          ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
-          truncated_command = command_* ratio ;
-          truncated_command = max(MIN_MOVING_SPEED,truncated_command);
-          joint_velocity_controller_.setCommand(truncated_command);
-        }
-        else if (position<=lower_deceleration_zone_)
-        {
-          ratio =  (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
-          truncated_command = command_* ratio;
-          truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
-          joint_velocity_controller_.setCommand(truncated_command);
-        }
-
-        if( position>= upper_turnaround_location_ ||
-          position<=lower_turnaround_location_ )
-        {
-          command_ = -command_; //Reverse direction
-          joint_velocity_controller_.setCommand(0);
-          passed_center_ = false; //Make sure we don't get stuck on edges of workspace
-        }
-      }
-      else
-      {
-
-         if(position >= upper_deceleration_zone_)
-        {
-          ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
-          truncated_command = command_* ratio;
-          truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
-          joint_velocity_controller_.setCommand(truncated_command);
-        }
-        else if (position<=lower_deceleration_zone_)
-        {
-          ratio =  (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
-          truncated_command = command_* ratio;
-          truncated_command = max(MIN_MOVING_SPEED,truncated_command);
-          joint_velocity_controller_.setCommand(truncated_command);
-        }
-
-      }
-    }
-
-    joint_velocity_controller_.update(); //Update lower controller
-      break;
-    case POSITION:
-      joint_position_controller_.setCommand(command_);
-      joint_position_controller_.update();
-      break;
-    default:
-      break;
-  }
-
-    last_time_ = time; //Keep track of last time for update
-    last_position_ = position;
-
-}
-
-void LaserScannerController::setJointEffort(double effort)
-{
-  joint_->commanded_effort_ = effort;
-}
-
-void LaserScannerController::setTurnaroundPoints(void)
-{
-  upper_turnaround_location_ = joint_->joint_->joint_limit_max_ - upper_turnaround_offset_;
-  upper_deceleration_zone_ = upper_turnaround_location_-upper_deceleration_buffer_;
-
-  lower_turnaround_location_ = joint_->joint_->joint_limit_min_ + lower_turnaround_offset_;
-  lower_deceleration_zone_ = lower_turnaround_location_ + lower_deceleration_buffer_;
-}
-
-ROS_REGISTER_CONTROLLER(LaserScannerControllerNode)
-LaserScannerControllerNode::LaserScannerControllerNode()
-{
-  c_ = new LaserScannerController();
-}
-
-
-LaserScannerControllerNode::~LaserScannerControllerNode()
-{
-  delete c_;
-}
-
-void LaserScannerControllerNode::update()
-{
-  c_->update();
-}
-
-// Return the measured joint position
-double LaserScannerControllerNode::getMeasuredPosition()
-{
-  return c_->getMeasuredPosition();
-}
-
-bool LaserScannerControllerNode::setCommand(
-  robot_mechanism_controllers::SetCommand::request &req,
-  robot_mechanism_controllers::SetCommand::response &resp)
-{
-
-  c_->current_mode_ = LaserScannerController::VELOCITY;
-  c_->setCommand(req.command);
-  resp.command = c_->getCommand();
-
-   return true;
-}
-
-bool LaserScannerControllerNode::getCommand(
-  robot_mechanism_controllers::GetCommand::request &req,
-  robot_mechanism_controllers::GetCommand::response &resp)
-{
-  resp.command = c_->getCommand();
-
-  return true;
-}
-
-bool LaserScannerControllerNode::setPosition(
-  robot_mechanism_controllers::SetPosition::request &req,
-  robot_mechanism_controllers::SetPosition::response &resp)
-{
-
-  c_->current_mode_ = LaserScannerController::POSITION;
-  c_->setCommand(req.position);
-  resp.command = c_->getCommand();
-
-   return true;
-}
-
-bool LaserScannerControllerNode::getPosition(
-  robot_mechanism_controllers::GetPosition::request &req,
-  robot_mechanism_controllers::GetPosition::response &resp)
-{
-  c_->current_mode_ =  LaserScannerController::POSITION;
-  resp.command = c_->getCommand();
-
-  return true;
-}
-
-
-bool LaserScannerControllerNode::setProfile(
-  robot_mechanism_controllers::SetProfile::request &req,
-  robot_mechanism_controllers::SetProfile::response &resp)
-{
-  setProfileCall(req.UpperTurnaround,req.LowerTurnaround, req.upperDecelBuffer, req.lowerDecelBuffer);
-  resp.time = c_->getTime();
-  return true;
-}
-
-bool LaserScannerControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
-{
-  ros::node *node = ros::node::instance();
-  string prefix = config->Attribute("name");
-
-  if (!c_->initXml(robot, config))
-    return false;
-  node->advertise_service(prefix + "/set_command", &LaserScannerControllerNode::setCommand, this);
-  node->advertise_service(prefix + "/get_command", &LaserScannerControllerNode::getCommand, this);
-  node->advertise_service(prefix + "/set_profile", &LaserScannerControllerNode::setProfile, this);
-  node->advertise_service(prefix + "/set_position", &LaserScannerControllerNode::setPosition, this);
-  node->advertise_service(prefix + "/get_position", &LaserScannerControllerNode::getPosition, this);
-  return true;
-}
-
-bool LaserScannerControllerNode::setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer)
-{
-  c_->upper_turnaround_offset_ = upper_turn_around;
-  c_->lower_turnaround_offset_ = lower_turn_around;
-  c_->lower_deceleration_buffer_ = upper_decel_buffer;
-  c_->upper_deceleration_buffer_ = lower_decel_buffer;
-  c_->setTurnaroundPoints();
-
-  if(upper_turn_around == 0 && lower_turn_around ==0) c_->automatic_turnaround_ = false;
-  else c_->automatic_turnaround_ = true;
-  c_->current_mode_ = LaserScannerController::VELOCITY;
-  c_->setCommand(0.0);
-
-  return true;
-}
-
-bool LaserScannerControllerNode::getActual(
-  robot_mechanism_controllers::GetActual::request &req,
-  robot_mechanism_controllers::GetActual::response &resp)
-{
-  resp.command = c_->getMeasuredPosition();
-  resp.time = c_->getTime();
-  return true;
-}
-
-void LaserScannerControllerNode::setCommand(double command)
-{
-  c_->setCommand(command);
-}
-
-// Return the current position command
-double LaserScannerControllerNode::getCommand()
-{
-  return c_->getCommand();
-}
-
-
-

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp (from rev 4814, pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller_david.cpp)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_velocity_controller.cpp	2008-09-30 20:48:22 UTC (rev 4833)
@@ -0,0 +1,377 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include <algorithm>
+
+#include <pr2_mechanism_controllers/laser_scanner_velocity_controller.h>
+#include <math_utils/angles.h>
+#include <math_utils/velocity.h>
+using namespace std;
+using namespace controller;
+#define MIN_MOVING_SPEED 0.3
+ROS_REGISTER_CONTROLLER(LaserScannerVelocityController)
+
+LaserScannerVelocityController::LaserScannerVelocityController()
+{
+  robot_ = NULL;
+  joint_ = NULL;
+
+  command_ = 0;
+  last_time_ = 0;
+  current_mode_ = VELOCITY; //Start out in velocity mode
+
+
+  passed_center_ = true;
+  upper_turnaround_offset_ = 0.0;
+  lower_turnaround_offset_ = 0.0;
+  automatic_turnaround_ = false;
+  upper_deceleration_buffer_ = 0.0;
+  lower_deceleration_buffer_ = 0.0;
+}
+
+LaserScannerVelocityController::~LaserScannerVelocityController()
+{
+
+}
+
+void LaserScannerVelocityController::init(double p_gain, double i_gain, double d_gain, double windup, double time, std::string name, mechanism::RobotState *robot)
+{
+  robot_ = robot;
+  joint_ = robot->getJointState(name);
+
+  abort(); //joint_position_controller_.init( p_gain,  i_gain,  d_gain,  windup, time, name, robot);
+  command_= 0;
+  last_time_= time;
+}
+
+bool LaserScannerVelocityController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  assert(robot);
+
+
+  robot_ = robot;
+  last_time_ = robot->hw_->current_time_;
+
+  TiXmlElement *vel = config->FirstChildElement("velocity");
+
+  //Perform checks at highest level to give the most informative error message possible
+  TiXmlElement *j = vel->FirstChildElement("joint");
+  if (!j)
+  {
+    fprintf(stderr, "LaserScannerController was not given a joint\n");
+    return false;
+  }
+
+  const char *joint_name = j->Attribute("name");
+  int index = joint_name ? robot->model_->getJointIndex(joint_name) : -1;
+
+  if (index < 0)
+  {
+    fprintf(stderr, "JointPositionController could not find joint named \"%s\"\n", joint_name);
+    return false;
+  }
+  joint_ = &robot->joint_states_[index];
+
+ TiXmlElement *p = j->FirstChildElement("pid");
+  if (!p)
+    fprintf(stderr, "LaserScannerController's config did not specify the default pid parameters.\n");
+
+  joint_velocity_controller_.initXml(robot,vel); //Pass down XML snippet to encapsulated joint_velocity_controller_
+
+  TiXmlElement *pos = config->FirstChildElement("position");
+
+  joint_position_controller_.initXml(robot,pos); //Pass down XML snippet to encapsulated joint_position_controller_
+
+  last_position_ = joint_->position_;
+  return true;
+ }
+
+// Set the joint velocity command
+void LaserScannerVelocityController::setCommand(double command)
+{
+  command_ = command;
+  if(current_mode_ == VELOCITY && automatic_turnaround_)
+  {
+    if(joint_->position_> upper_turnaround_location_)
+    {
+      if(command_ > 0) command_ = -command_; //Turn command around to avoid crash
+      passed_center_ = false; //Allow passing through
+    }
+    else if (joint_->position_ < lower_turnaround_location_)
+    {
+      if(command_<0) command_ = -command_;
+      passed_center_ = false;
+    }
+  }
+}
+
+// Return the current position command
+double LaserScannerVelocityController::getCommand()
+{
+  return command_;
+}
+
+// Return the measured joint position
+double LaserScannerVelocityController::getMeasuredPosition()
+{
+  return joint_->position_;
+}
+
+double LaserScannerVelocityController::getTime()
+{
+  return robot_->hw_->current_time_;
+}
+
+void LaserScannerVelocityController::update()
+{
+  double time = robot_->hw_->current_time_;
+  double position = joint_->position_;
+  double truncated_command, ratio;
+
+  switch(current_mode_)
+  {
+    case VELOCITY:
+
+     joint_velocity_controller_.setCommand(command_);
+
+    if(automatic_turnaround_) //Automatically turn around
+    {
+       //Track if we passed center point
+      if((position>=0 && last_position_<=0) || (position<=0 && last_position_>=0))  passed_center_ = true;
+
+      if(passed_center_) //Check whether we're moving towards the edge
+      {
+
+        if(position >= upper_deceleration_zone_)
+        {
+          //Scale command for deceleration
+          ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
+          truncated_command = command_* ratio ;
+          truncated_command = max(MIN_MOVING_SPEED,truncated_command);
+          joint_velocity_controller_.setCommand(truncated_command);
+        }
+        else if (position<=lower_deceleration_zone_)
+        {
+          ratio =  (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
+          truncated_command = command_* ratio;
+          truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
+          joint_velocity_controller_.setCommand(truncated_command);
+        }
+
+        if( position>= upper_turnaround_location_ ||
+          position<=lower_turnaround_location_ )
+        {
+          command_ = -command_; //Reverse direction
+          joint_velocity_controller_.setCommand(0);
+          passed_center_ = false; //Make sure we don't get stuck on edges of workspace
+        }
+      }
+      else
+      {
+
+         if(position >= upper_deceleration_zone_)
+        {
+          ratio = (1-(position-upper_deceleration_zone_)/(upper_turnaround_location_-upper_deceleration_zone_));
+          truncated_command = command_* ratio;
+          truncated_command = min(-MIN_MOVING_SPEED,truncated_command);
+          joint_velocity_controller_.setCommand(truncated_command);
+        }
+        else if (position<=lower_deceleration_zone_)
+        {
+          ratio =  (1-(lower_deceleration_zone_ - position)/(lower_deceleration_zone_-lower_turnaround_location_));
+          truncated_command = command_* ratio;
+          truncated_command = max(MIN_MOVING_SPEED,truncated_command);
+          joint_velocity_controller_.setCommand(truncated_command);
+        }
+
+      }
+    }
+
+    joint_velocity_controller_.update(); //Update lower controller
+      break;
+    case POSITION:
+      joint_position_controller_.setCommand(command_);
+      joint_position_controller_.update();
+      break;
+    default:
+      break;
+  }
+
+    last_time_ = time; //Keep track of last time for update
+    last_position_ = position;
+
+}
+
+void LaserScannerVelocityController::setJointEffort(double effort)
+{
+  joint_->commanded_effort_ = effort;
+}
+
+void LaserScannerVelocityController::setTurnaroundPoints(void)
+{
+  upper_turnaround_location_ = joint_->joint_->joint_limit_max_ - upper_turnaround_offset_;
+  upper_deceleration_zone_ = upper_turnaround_location_-upper_deceleration_buffer_;
+
+  lower_turnaround_location_ = joint_->joint_->joint_limit_min_ + lower_turnaround_offset_;
+  lower_deceleration_zone_ = lower_turnaround_location_ + lower_deceleration_buffer_;
+}
+
+ROS_REGISTER_CONTROLLER(LaserScannerVelocityControllerNode)
+LaserScannerVelocityControllerNode::LaserScannerVelocityControllerNode()
+{
+  c_ = new LaserScannerVelocityController();
+}
+
+
+LaserScannerVelocityControllerNode::~LaserScannerVelocityControllerNode()
+{
+  delete c_;
+}
+
+void LaserScannerVelocityControllerNode::update()
+{
+  c_->update();
+}
+
+// Return the measured joint position
+double LaserScannerVelocityControllerNode::getMeasuredPosition()
+{
+  return c_->getMeasuredPosition();
+}
+
+bool LaserScannerVelocityControllerNode::setCommand(
+  robot_mechanism_controllers::SetCommand::request &req,
+  robot_mechanism_controllers::SetCommand::response &resp)
+{
+
+  c_->current_mode_ = LaserScannerVelocityController::VELOCITY;
+  c_->setCommand(req.command);
+  resp.command = c_->getCommand();
+
+   return true;
+}
+
+bool LaserScannerVelocityControllerNode::getCommand(
+  robot_mechanism_controllers::GetCommand::request &req,
+  robot_mechanism_controllers::GetCommand::response &resp)
+{
+  resp.command = c_->getCommand();
+
+  return true;
+}
+
+bool LaserScannerVelocityControllerNode::setPosition(
+  robot_mechanism_controllers::SetPosition::request &req,
+  robot_mechanism_controllers::SetPosition::response &resp)
+{
+
+  c_->current_mode_ = LaserScannerVelocityController::POSITION;
+  c_->setCommand(req.position);
+  resp.command = c_->getCommand();
+
+   return true;
+}
+
+bool LaserScannerVelocityControllerNode::getPosition(
+  robot_mechanism_controllers::GetPosition::request &req,
+  robot_mechanism_controllers::GetPosition::response &resp)
+{
+  c_->current_mode_ =  LaserScannerVelocityController::POSITION;
+  resp.command = c_->getCommand();
+
+  return true;
+}
+
+
+bool LaserScannerVelocityControllerNode::setProfile(
+  pr2_mechanism_controllers::SetProfile::request &req,
+  pr2_mechanism_controllers::SetProfile::response &resp)
+{
+  setProfileCall(req.UpperTurnaround,req.LowerTurnaround, req.upperDecelBuffer, req.lowerDecelBuffer);
+  resp.time = c_->getTime();
+  return true;
+}
+
+bool LaserScannerVelocityControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  ros::node *node = ros::node::instance();
+  string prefix = config->Attribute("name");
+
+  if (!c_->initXml(robot, config))
+    return false;
+  node->advertise_service(prefix + "/set_command", &LaserScannerVelocityControllerNode::setCommand, this);
+  node->advertise_service(prefix + "/get_command", &LaserScannerVelocityControllerNode::getCommand, this);
+  node->advertise_service(prefix + "/set_profile", &LaserScannerVelocityControllerNode::setProfile, this);
+  node->advertise_service(prefix + "/set_position", &LaserScannerVelocityControllerNode::setPosition, this);
+  node->advertise_service(prefix + "/get_position", &LaserScannerVelocityControllerNode::getPosition, this);
+  return true;
+}
+
+bool LaserScannerVelocityControllerNode::setProfileCall(double upper_turn_around, double lower_turn_around, double upper_decel_buffer, double lower_decel_buffer)
+{
+  c_->upper_turnaround_offset_ = upper_turn_around;
+  c_->lower_turnaround_offset_ = lower_turn_around;
+  c_->lower_deceleration_buffer_ = upper_decel_buffer;
+  c_->upper_deceleration_buffer_ = lower_decel_buffer;
+  c_->setTurnaroundPoints();
+
+  if(upper_turn_around == 0 && lower_turn_around ==0) c_->automatic_turnaround_ = false;
+  else c_->automatic_turnaround_ = true;
+  c_->current_mode_ = LaserScannerVelocityController::VELOCITY;
+  c_->setCommand(0.0);
+
+  return true;
+}
+
+bool LaserScannerVelocityControllerNode::getActual(
+  robot_mechanism_controllers::GetActual::request &req,
+  robot_mechanism_controllers::GetActual::response &resp)
+{
+  resp.command = c_->getMeasuredPosition();
+  resp.time = c_->getTime();
+  return true;
+}
+
+void LaserScannerVelocityControllerNode::setCommand(double command)
+{
+  c_->setCommand(command);
+}
+
+// Return the current position command
+double LaserScannerVelocityControllerNode::getCommand()
+{
+  return c_->getCommand();
+}
+
+
+


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 4954
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4954&view=rev
Author:   vijaypradeep
Date:     2008-10-02 20:41:26 +0000 (Thu, 02 Oct 2008)

Log Message:
-----------
Laser Scanner controller now sends a message when halfway through a sweep, and when done with a sweep.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h	2008-10-02 20:23:39 UTC (rev 4953)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h	2008-10-02 20:41:26 UTC (rev 4954)
@@ -39,6 +39,11 @@
 #include <mechanism_model/controller.h>
 #include <robot_mechanism_controllers/joint_position_controller.h>
 
+#include <misc_utils/realtime_publisher.h>
+
+// Messages
+#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+
 // Services
 #include <robot_mechanism_controllers/SetCommand.h>
 #include <robot_mechanism_controllers/GetCommand.h>
@@ -56,6 +61,14 @@
     MANUAL,SAWTOOTH,SINEWAVE,DYNAMIC_SAWTOOTH,DYNAMIC_SINEWAVE,AUTO_LEVEL
   };
 
+  //! Used to specify which section of a profile we are currently in.
+  enum ProfileExecutionState
+  {
+    NOT_APPLICABLE = 0,  //!< Implies that ProfileExecutionState doesn't make sense in our current control mode
+    FIRST_HALF  = 1,     //!< Specifies that we're in the first half of our current profile
+    SECOND_HALF = 2      //!< Specifies that we're in the second half of our current profile
+  } ;
+  
   /*!
    * \brief Default Constructor of the JointController class.
    *
@@ -151,6 +164,13 @@
    */
   bool checkAutoLevelResult();
 
+  /*!
+   * \brief Get which half of the current profile we're in.
+   * \return The current profileExecutionState. Will be either first or second half. \
+   *         Will return NotApplicable if we're not currently following a profile
+   */
+  ProfileExecutionState getProfileExecutionState() ;
+  
   double getTime();
 private:
   /*!
@@ -201,7 +221,7 @@
    *
    */
   LaserScannerControllerNode();
-
+  
   /*!
    * \brief Destructor
    */
@@ -221,17 +241,17 @@
                   robot_mechanism_controllers::GetCommand::response &resp);
 
   bool getActual(robot_mechanism_controllers::GetActual::request &req,
-                  robot_mechanism_controllers::GetActual::response &resp);
+                 robot_mechanism_controllers::GetActual::response &resp);
 
   bool setProfileCall(pr2_mechanism_controllers::SetProfile::request &req,
-  pr2_mechanism_controllers::SetProfile::response &resp);
+                      pr2_mechanism_controllers::SetProfile::response &resp);
 
   void setCommand(double command);
 
   void setProfile(LaserScannerController::LaserControllerMode profile, double period, double amplitude, int num_elements=0, double offset=0.0);
 
   double getCommand();
-
+  
 private:
   LaserScannerController *c_;
   /*!
@@ -243,7 +263,11 @@
    * \brief A pointer to ros node
    */
   ros::node *node_;
-
+  
+  LaserScannerController::ProfileExecutionState prev_profile_exec_state_ ;       //!< Store the previous profileExecutionState. Need this to compare to the current state to detect transitions
+  pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ;              //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
+  bool need_to_send_msg_ ;                                                       //!< Tracks whether we still need to send out the m_scanner_signal_ message.
+  misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
 };
 }
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp	2008-10-02 20:23:39 UTC (rev 4953)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp	2008-10-02 20:41:26 UTC (rev 4954)
@@ -34,6 +34,7 @@
 #include <algorithm>
 
 #include <pr2_mechanism_controllers/laser_scanner_controller.h>
+
 #include <math_utils/angles.h>
 
 using namespace std;
@@ -154,11 +155,11 @@
         time_of_last_point_ = time;
 
         //Advance time index
-    		 if(profile_index_ == (profile_length_-1))
-         {
-  			  profile_index_ = 0; //Restart profile
-  			  cycle_start_time_ = time;
-  			} else profile_index_++;
+        if(profile_index_ == (profile_length_-1))
+        {
+          profile_index_ = 0; //Restart profile
+          cycle_start_time_ = time;
+        } else profile_index_++;
       }
       break;
     case DYNAMIC_SAWTOOTH:
@@ -360,7 +361,10 @@
 //Get sinewave based on current time
 void LaserScannerController::setDynamicSinewave(double time_from_start)
 {
-  double command = sin(2*M_PI*time_from_start/period_)*amplitude_+offset_;
+  //double command = sin(2*M_PI*time_from_start/period_)*amplitude_+offset_;
+
+  // Switched to cosine, so that the first half and second half are each a sweep (as opposed to 2/4+3/4 and 1/4+4/4 being sweeps)
+  double command = cos(2*M_PI*time_from_start/period_)*amplitude_+offset_;
   joint_position_controller_.setCommand(command);
 
 }
@@ -391,10 +395,47 @@
   joint_position_controller_.setCommand(command);
 }
 
+LaserScannerController::ProfileExecutionState LaserScannerController::getProfileExecutionState()
+{
+  switch (current_mode_)
+  {
+    case SINEWAVE :
+    case SAWTOOTH :
+    {
+      int halfway_point = profile_length_ / 2 ;             // We should be able to get away with doing integer math here.
+      if (profile_index_ <= halfway_point)
+        return FIRST_HALF ;
+      else
+        return SECOND_HALF ;
+    }
+      
+    case DYNAMIC_SINEWAVE :
+    case DYNAMIC_SAWTOOTH :
+    {
+      double time = robot_->hw_->current_time_ ;
+      double time_from_start = time - cycle_start_time_ ;
+      
+      if ( time_from_start < period_ / 2.0 )
+        return FIRST_HALF ;
+      else
+        return SECOND_HALF ;                                 // It is possible that (time_from_start > period). These should return as FIRST_HALF, but at this point, we don't care.
+    }
+
+    case MANUAL :
+    case AUTO_LEVEL :
+      return NOT_APPLICABLE ;
+    default:
+      return NOT_APPLICABLE ;
+  }
+}
+
 ROS_REGISTER_CONTROLLER(LaserScannerControllerNode)
 LaserScannerControllerNode::LaserScannerControllerNode(): node_(ros::node::instance())
 {
   c_ = new LaserScannerController();
+  prev_profile_exec_state_ = LaserScannerController::NOT_APPLICABLE ;
+  need_to_send_msg_ = false ;                                           // Haven't completed a sweep yet, so don't need to send a msg
+  publisher_ = NULL ;                                                   // We don't know our topic yet, so we can't build it
 }
 
 
@@ -403,12 +444,64 @@
   node_->unadvertise_service(service_prefix_ + "/set_command");
   node_->unadvertise_service(service_prefix_ + "/get_command");
   node_->unadvertise_service(service_prefix_ + "/set_profile");
+  //node_->unadvertise(service_prefix_ + "/laser_scanner_signal") ;
+
+  publisher_->stop() ;
+  delete publisher_ ;    // Probably should wait on publish_->is_running() before exiting. Need to 
+                         //   look into shutdown semantics for realtime_publisher
   delete c_;
 }
 
 void LaserScannerControllerNode::update()
 {
   c_->update();
+  
+  LaserScannerController::ProfileExecutionState cur_profile_exec_state ;
+  cur_profile_exec_state = c_->getProfileExecutionState() ;
+  switch (prev_profile_exec_state_)
+  {
+    case LaserScannerController::FIRST_HALF :
+      if (cur_profile_exec_state == LaserScannerController::SECOND_HALF)   // We just transitioned from 1st->2nd, so send msg saying half-scan is done
+      {
+        m_scanner_signal_.signal = 0 ;                                     // 0 -> Half profile complete
+
+        if (need_to_send_msg_)                                             // Is there a message we were supposed to send, but never got to?
+          printf("LaserScannerController:: Missed sending a msg\n") ;      // Is there a better way to output this error msg?
+
+        need_to_send_msg_ = true ;
+      }
+      break ;
+    case LaserScannerController::SECOND_HALF :                            // Send msg saying full scan is done
+      if (cur_profile_exec_state == LaserScannerController::FIRST_HALF)   // We just transitioned from 2nd->1st, so send msg saying full-scan is done
+      {
+        m_scanner_signal_.signal = 1 ;                                    // 1 -> Full profile complete
+
+        if (need_to_send_msg_)                                             // Is there a message we were supposed to send, but never got to?
+          printf("LaserScannerController:: Missed sending a msg\n") ;      // Is there a better way to output this error msg?
+
+        need_to_send_msg_ = true ;
+      }
+      break ;
+    case LaserScannerController::NOT_APPLICABLE :                         // Don't do anything
+      break ;
+    default :
+      break ;
+  }
+  prev_profile_exec_state_ = cur_profile_exec_state ;
+  
+  // Use the realtime_publisher to try to send the message.
+  //   If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
+  if (need_to_send_msg_)
+  {
+    if (publisher_->trylock())
+    {
+      publisher_->msg_.signal = m_scanner_signal_.signal ;
+      publisher_->unlockAndPublish() ;
+      need_to_send_msg_ = false ;
+    }
+    //printf("tilt_laser: Signal trigger (%u)\n", m_scanner_signal_.signal) ;
+    //std::cout << std::flush ;
+  }
 }
 
 // Return the measured joint position
@@ -438,6 +531,10 @@
   else if (req.command==-45)c_->setSinewaveProfile(1,0.5,0);
   else if (req.command==-46)c_->setSinewaveProfile(3,0.5,0);
   else if (req.command==-47)c_->setSinewaveProfile(20,0.7,0);
+  else if (req.command==-48)c_->setSinewaveProfile(15,1.2,0.25);
+  else if (req.command==-49)c_->setSinewaveProfile(10,1.2,0.25);
+  else if (req.command==-50)c_->setSinewaveProfile(8,1.2,0.25);
+  else if (req.command==-51)c_->setSinewaveProfile(5,1.2,0.25);
   return true;
 }
 
@@ -468,6 +565,12 @@
   node_->advertise_service(service_prefix_ + "/set_command", &LaserScannerControllerNode::setCommand, this);
   node_->advertise_service(service_prefix_ + "/get_command", &LaserScannerControllerNode::getCommand, this);
   node_->advertise_service(service_prefix_ + "/set_profile", &LaserScannerControllerNode::setProfileCall, this);
+  //node_->advertise<pr2_mechanism_controllers::LaserScannerSignal>(service_prefix_ + "/laser_scanner_signal", 1) ;
+
+  if (publisher_ != NULL)               // Make sure that we don't memory leak if initXml gets called twice
+    delete publisher_ ;
+  publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+  
   return true;
 }
 bool LaserScannerControllerNode::getActual(


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5076
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5076&view=rev
Author:   sachinchitta
Date:     2008-10-06 17:33:06 +0000 (Mon, 06 Oct 2008)

Log Message:
-----------
Added velocity limits to base controller

Arm dynamics now works

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	2008-10-06 16:56:40 UTC (rev 5075)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_dynamics_controller.h	2008-10-06 17:33:06 UTC (rev 5076)
@@ -38,6 +38,7 @@
 #include <rosthread/mutex.h>
 
 #include <mechanism_model/controller.h>
+#include <mechanism_model/joint.h>
 #include <robot_mechanism_controllers/joint_position_controller.h>
 #include <robot_mechanism_controllers/joint_velocity_controller.h>
 #include <robot_mechanism_controllers/joint_effort_controller.h>
@@ -52,6 +53,9 @@
 //Kinematics
 #include <robot_kinematics/robot_kinematics.h>
 
+// Math utils
+#include <math_utils/angles.h>
+
 // #include <libTF/Pose3D.h>
 // #include <urdf/URDF.h>
 
@@ -125,8 +129,14 @@
 
   robot_kinematics::SerialChain *arm_chain_;
 
+  unsigned int num_joints_;
+
 private:
 
+  double last_time_;
+
+  std::vector<control_toolbox::Pid> pid_controllers_;
+
   std::vector<JointEffortController *> joint_effort_controllers_;
 
   // Goal of the joints
@@ -157,9 +167,12 @@
 
   bool goal_achieved_;
 
-  KDL::Vector *kdl_torque_;
+  KDL::Vector *gravity_torque_;
 
+  std::vector<double> control_torque_;
 
+  void computeControlTorque(const double &time);
+
   // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
   bool refresh_rt_vals_;
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-06 16:56:40 UTC (rev 5075)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-06 17:33:06 UTC (rev 5076)
@@ -324,6 +324,12 @@
     double cmd_received_timestamp_;
 
     double timeout_;
+
+    double max_x_dot_;
+
+    double max_y_dot_;
+
+    double max_yaw_dot_;
   };
 
   class BaseControllerNode : public Controller

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-10-06 16:56:40 UTC (rev 5075)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-10-06 17:33:06 UTC (rev 5076)
@@ -41,17 +41,18 @@
 
 ROS_REGISTER_CONTROLLER(ArmDynamicsController);
 
-ArmDynamicsController::ArmDynamicsController()
+ArmDynamicsController::ArmDynamicsController():num_joints_(0),last_time_(0)
 {
 }
 
 ArmDynamicsController::~ArmDynamicsController()
 {
   // Assumes the joint controllers are owned by this class.
-  for(unsigned int i=0; i<joint_effort_controllers_.size();++i)
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
     delete joint_effort_controllers_[i];
-
-  delete kdl_torque_;
+  }
+  delete gravity_torque_;
 }
 
 bool ArmDynamicsController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
@@ -60,26 +61,46 @@
   TiXmlElement *elt = config->FirstChildElement("controller");
   while (elt)
   {
-    JointEffortController * jpc = new JointEffortController();
+    JointEffortController * jec = new JointEffortController();
     std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
     assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointEffortController"));
-    joint_effort_controllers_.push_back(jpc);
-    if(!jpc->initXml(robot, elt))
+    joint_effort_controllers_.push_back(jec);
+    if(!jec->initXml(robot, elt))
       return false;
 
+    TiXmlElement *p = elt->FirstChildElement("joint")->FirstChildElement("pid");
+    if (p)
+    {
+      control_toolbox::Pid  pid_controller;
+      pid_controller.initPid(0, 0, 0, 0, 0);
+      pid_controller.initXml(p);
+      pid_controllers_.push_back(pid_controller);
+   }
+    else
+      fprintf(stderr, "JointEffortController's config did not specify the default pid parameters.\n");
+
     elt = elt->NextSiblingElement("controller");
   }
-  goals_.resize(joint_effort_controllers_.size());
-  goals_rt_.resize(joint_effort_controllers_.size());
 
-  goals_dot_.resize(joint_effort_controllers_.size());
-  goals_rt_dot_.resize(joint_effort_controllers_.size());
+  num_joints_ = joint_effort_controllers_.size();
 
-  goals_dot_dot_.resize(joint_effort_controllers_.size());
-  goals_rt_dot_dot_.resize(joint_effort_controllers_.size());
+  fprintf(stderr,"ArmDynamicsController:: num_joints_: %d\n",num_joints_);
 
-  kdl_torque_ = new KDL::Vector[joint_effort_controllers_.size()+1];
+  goals_.resize(num_joints_);
+  goals_rt_.resize(num_joints_);
 
+  goals_dot_.resize(num_joints_);
+  goals_rt_dot_.resize(num_joints_);
+
+  goals_dot_dot_.resize(num_joints_);
+  goals_rt_dot_dot_.resize(num_joints_);
+
+  control_torque_.resize(num_joints_);
+
+  gravity_torque_ = new KDL::Vector[num_joints_+1];
+
+  last_time_ = robot->hw_->current_time_;
+
   return true;
 }
 
@@ -118,7 +139,7 @@
 
 controller::JointEffortController* ArmDynamicsController::getJointEffortControllerByName(std::string name)
 {
-  for(int i=0; i< (int) joint_effort_controllers_.size(); i++)
+  for(int i=0; i< (int) num_joints_; i++)
   {
     if(joint_effort_controllers_[i]->getJointName() == name)
     {
@@ -131,7 +152,7 @@
 
 int ArmDynamicsController::getJointControllerByName(std::string name)
 {
-  for(int i=0; i< (int) joint_effort_controllers_.size(); i++)
+  for(int i=0; i< (int) num_joints_; i++)
   {
     if(joint_effort_controllers_[i]->getJointName() == name)
     {
@@ -144,15 +165,12 @@
 
 void ArmDynamicsController::update(void)
 {
-   cout << "Updating dynamics controller " << std::endl;
-   KDL::JntArray kdl_q(goals_rt_.size());
-   KDL::JntArray kdl_q_dot(goals_rt_.size());
-   KDL::JntArray kdl_q_dot_dot(goals_rt_.size());
+//  cout << "Updating dynamics controller " << std::endl;
+  double time = robot_->hw_->current_time_;
 
   if(refresh_rt_vals_ && arm_controller_lock_.trylock())
   {
-    assert(goals_.size() == goals_rt_.size());
-    for(unsigned int i=0; i<goals_.size(); ++i)
+    for(unsigned int i=0; i < num_joints_; ++i)
     {
       goals_rt_[i] = goals_[i];
       goals_rt_dot_[i] = goals_dot_[i];
@@ -161,32 +179,65 @@
     arm_controller_lock_.unlock();
   }
 
-    for(unsigned int i=0; i<goals_.size(); ++i)
-    {
-//       kdl_q(i) = joint_effort_controllers_[i]->joint_->position_;
-       kdl_q(i) = 0.0;
-       kdl_q_dot(i) = 0.0;
-       kdl_q_dot_dot(i) = 0.0;
-    }
+  computeControlTorque(time);
 
-    arm_chain_->computeInverseDynamics(kdl_q,kdl_q_dot,kdl_q_dot_dot,kdl_torque_);
-
-  for(unsigned int i=0;i<goals_rt_.size();++i)
+  for(unsigned int i=0; i < num_joints_;++i)
   {
-      joint_effort_controllers_[i]->setCommand(3.5*kdl_torque_[i][2]);
-      //  joint_effort_controllers_[i]->setCommand(0.0);
-      fprintf(stderr,"Effort: %d %f %f\n",i,kdl_torque_[i][2],joint_effort_controllers_[i]->joint_->commanded_effort_);
+   joint_effort_controllers_[i]->setCommand(control_torque_[i]);
+//    joint_effort_controllers_[i]->setCommand(0.0);
   }
+
   updateJointControllers();
-  for(unsigned int i=0;i<goals_rt_.size();++i)
+
+  for(unsigned int i=0; i < num_joints_; ++i)
   {
-      fprintf(stderr,"Effort: %d %f %f\n",i,kdl_torque_[i][2],joint_effort_controllers_[i]->joint_->commanded_effort_);
+//    fprintf(stderr,"Effort: %d %f %f %f\n",i,control_torque_[i],gravity_torque_[i][2],joint_effort_controllers_[i]->joint_->commanded_effort_);
   }
 }
 
+void ArmDynamicsController::computeControlTorque(const double &time)
+{
+  KDL::JntArray kdl_q(num_joints_);
+  KDL::JntArray kdl_q_dot(num_joints_);
+  KDL::JntArray kdl_q_dot_dot(num_joints_);
+  double error(0),command(0),actual(0),pid_torque(0);
+  int j_type;
+
+  for(unsigned int i=0; i < num_joints_; ++i)
+  {
+    kdl_q(i) = joint_effort_controllers_[i]->joint_->position_;     
+  }  
+  arm_chain_->computeGravityTerms(kdl_q,gravity_torque_);
+
+  for(unsigned int i=0; i < num_joints_; ++i)
+  {
+    command = goals_rt_[i];
+    actual = joint_effort_controllers_[i]->joint_->position_;
+    j_type = joint_effort_controllers_[i]->joint_->joint_->type_;
+    if(j_type == mechanism::JOINT_ROTARY)
+    {
+      if(!math_utils::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_->joint_->joint_limit_max_,error))
+        error = 0;
+    }
+    else if(j_type == mechanism::JOINT_CONTINUOUS)
+    {
+      error = math_utils::shortest_angular_distance(command, actual);
+    }
+    else
+    {
+      error = actual - command;
+    }
+    pid_torque = pid_controllers_[i].updatePid(error, time - last_time_);
+    control_torque_[i] = gravity_torque_[i][2] + pid_torque;
+    fprintf(stderr,"%d:: %f, %f, %f, %f, %f, %f, %f\n",i,actual,command,error,pid_torque,gravity_torque_[i][2],control_torque_[i],time-last_time_);
+  }
+  last_time_ = time;
+}
+
+
 void ArmDynamicsController::updateJointControllers(void)
 {
-  for(unsigned int i=0;i<goals_rt_.size();++i)
+  for(unsigned int i=0;i<num_joints_;++i)
     joint_effort_controllers_[i]->update();
 }
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-06 16:56:40 UTC (rev 5075)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-06 17:33:06 UTC (rev 5076)
@@ -34,6 +34,7 @@
 
 #include <pr2_mechanism_controllers/base_controller.h>
 #include <math_utils/angles.h>
+#include <math_utils/math_utils.h>
 
 using namespace std;
 using namespace controller;
@@ -74,9 +75,10 @@
 void BaseController::setCommand(libTF::Vector cmd_vel)
 {
   pthread_mutex_lock(&base_controller_lock_);
-  cmd_vel_t_.x = cmd_vel.x;
-  cmd_vel_t_.y = cmd_vel.y;
-  cmd_vel_t_.z = cmd_vel.z;
+
+  cmd_vel_t_.x = clamp(cmd_vel.x,-max_x_dot_, max_x_dot_);
+  cmd_vel_t_.y = clamp(cmd_vel.y,-max_y_dot_, max_y_dot_);
+  cmd_vel_t_.z = clamp(cmd_vel.z,-max_yaw_dot_,max_yaw_dot_);
   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 //  std::cout << "command received : " << cmd_vel_t_ << std::endl;
   pthread_mutex_unlock(&base_controller_lock_);
@@ -236,6 +238,18 @@
         {
            timeout_ = atof(elt_key->GetText());
         }
+        if(elt_key->Attribute("key") == std::string("max_x_dot"))
+        {
+           max_x_dot_ = atof(elt_key->GetText());
+        }
+        if(elt_key->Attribute("key") == std::string("max_y_dot"))
+        {
+           max_y_dot_ = atof(elt_key->GetText());
+        }
+        if(elt_key->Attribute("key") == std::string("max_yaw_dot"))
+        {
+           max_yaw_dot_ = atof(elt_key->GetText());
+        }
         elt_key = elt_key->NextSiblingElement("elem");
       }
     }
@@ -515,7 +529,8 @@
 
   c_->setOdomMessage(odom_msg_);
 
-  if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
+  if(0)
+//  if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
   {
     (ros::g_node)->publish("odom", odom_msg_);
     odom_publish_counter_ = 0;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5090
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5090&view=rev
Author:   mmwise
Date:     2008-10-06 22:29:09 +0000 (Mon, 06 Oct 2008)

Log Message:
-----------
adding a simple head_pan_tilt_controller without transform

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-06 22:08:07 UTC (rev 5089)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-06 22:29:09 UTC (rev 5090)
@@ -3,7 +3,7 @@
 rospack(pr2_mechanism_controllers)
 genmsg()
 gensrv()
-#rospack_add_library(pr2_controllers src/base_controller.cpp src/laser_scanner_controller.cpp  src/arm_position_controller.cpp src/arm_velocity_controller.cpp src/arm_pd_controller.cpp)
+#rospack_add_library(pr2_controllers src/base_controller.cpp src/laser_scanner_controller.cpp  src/arm_position_controller.cpp src/arm_velocity_controller.cpp src/arm_pd_controller.cpp src/head_pan_tilt_controller.cpp)
 
 rospack_add_library( pr2_mechanism_controllers
   src/base_controller.cpp
@@ -11,7 +11,8 @@
   src/laser_scanner_velocity_controller.cpp
   src/arm_position_controller.cpp
   src/arm_velocity_controller.cpp
-  src/arm_dynamics_controller.cpp)
+  src/arm_dynamics_controller.cpp
+  src/head_pan_tilt_controller.cpp)
 
 rospack_add_executable(testBaseController test/test_base_controller.cpp)
 target_link_libraries(testBaseController pr2_mechanism_controllers)

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-06 22:29:09 UTC (rev 5090)
@@ -0,0 +1,197 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+#include <mechanism_model/controller.h>
+#include <mechanism_model/joint.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+
+// Services
+#include <pr2_mechanism_controllers/SetJointCmd.h>
+#include <pr2_mechanism_controllers/GetJointCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointGains.h>
+#include <pr2_mechanism_controllers/GetJointGains.h>
+
+//Kinematics
+#include <robot_kinematics/robot_kinematics.h>
+
+// Math utils
+#include <math_utils/angles.h>
+
+// #include <libTF/Pose3D.h>
+// #include <urdf/URDF.h>
+
+namespace controller
+{
+
+// The maximum number of joints expected in an arm.
+static const int MAX_HEAD_JOINTS = 2;
+
+class HeadPanTiltController : public Controller
+{
+public:
+
+  /*!
+   * \brief Default Constructor of the JointController class.
+   *
+   */
+  HeadPanTiltController();
+
+  /*!
+   * \brief Destructor of the JointController class.
+   */
+  virtual ~HeadPanTiltController();
+
+  /*!
+   * \brief Functional way to initialize limits and gains.
+   *
+  */
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+  /*!
+   * \brief Sets a goal for some joint, specified as a pair of (joint values, joint names)
+   *
+   * \param j_values the positions of the joints
+   * \param j_names the names of the joints
+  */
+  void setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names);
+
+//  /**
+//   * @brief Overloaded method for convenience
+//   * @param req
+//   */
+//  void setJointCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
+
+  void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
+
+  /*!
+   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+  */
+//  void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+
+//  void getCurrentConfiguration(std::vector<double> &);
+
+  /*!
+     * \brief Issues commands to the joint. Should be called at regular intervals
+  */
+  virtual void update(void); // Real time safe.
+
+  controller::JointPositionController* getJointPositionControllerByName(std::string name);
+
+  unsigned int num_joints_;
+
+private:
+
+  double last_time_;
+
+  std::vector<JointPositionController *> joint_position_controllers_;
+
+  // Goal of the joints
+  std::vector<double> set_pts_;
+
+  mechanism::Robot* robot_;
+
+  void updateJointControllers(void);
+
+  int getJointControllerByName(std::string name);
+
+
+};
+
+/** @class HeadPanTiltControllerNode
+ *  @brief ROS interface for the head controller.
+ *
+ *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the head:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
+ *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
+ *
+ */
+class HeadPanTiltControllerNode : public Controller
+{
+  public:
+    /*!
+    * \brief Default Constructor
+    *
+    */
+    HeadPanTiltControllerNode();
+
+    /*!
+    * \brief Destructor
+    */
+    ~HeadPanTiltControllerNode();
+
+    void update();
+
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /** @brief sets a command for all the joints managed by the controller at once
+     * This is a lightweight version of setJointPosHeadless when the caller knows the order of the joints.
+     * @note this service should not be used. use setJointPosHeadless instead
+     * This service returns immediately
+     * @param req an array of position
+     * @param resp The response is empty
+     * @return
+     */
+    bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+                    pr2_mechanism_controllers::SetJointCmd::response &resp);
+
+    /** @brief service that returns the goal of the controller
+     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
+     * @param req
+     * @param resp the response, contains a JointPosCmd message with the goal of the controller
+     * @return
+     */
+    bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
+                        pr2_mechanism_controllers::GetJointCmd::response &resp);
+
+  private:
+    pr2_mechanism_controllers::JointCmd msg_;   //The message used by the ROS callback
+    HeadPanTiltController *c_;
+
+};
+
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-06 22:29:09 UTC (rev 5090)
@@ -0,0 +1,213 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Melonee Wise <mwise@...>
+
+#include "pr2_mechanism_controllers/head_pan_tilt_controller.h"
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(HeadPanTiltController);
+
+HeadPanTiltController::HeadPanTiltController():num_joints_(0),last_time_(0)
+{
+}
+
+HeadPanTiltController::~HeadPanTiltController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
+    delete joint_position_controllers_[i];
+  }
+}
+
+bool HeadPanTiltController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  robot_ = robot->model_;
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    JointPositionController * jpc = new JointPositionController();
+    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPositionController"));
+    joint_position_controllers_.push_back(jpc);
+    if(!jpc->initXml(robot, elt))
+      return false;
+
+    elt = elt->NextSiblingElement("controller");
+  }
+
+  num_joints_ = joint_position_controllers_.size();
+
+  fprintf(stderr,"HeadPanTiltController:: num_joints_: %d\n",num_joints_);
+
+  set_pts_.resize(num_joints_);
+  last_time_ = robot->hw_->current_time_;
+
+  return true;
+}
+
+void HeadPanTiltController::setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names)
+{
+  assert(j_values.size() == j_names.size());
+  for(uint i = 0; i < j_values.size(); ++i)
+  {
+    const std::string & name = j_names[i];
+    const int id = getJointControllerByName(name);
+    if(id >= 0)
+    {
+      set_pts_[id] = j_values[i];
+    }
+  }
+}
+
+
+void HeadPanTiltController::getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const
+{
+  const unsigned int n = joint_position_controllers_.size();
+  cmd.set_names_size(n);
+  for(unsigned int i=0; i<n; ++i)
+      cmd.names[i] = joint_position_controllers_[i]->getJointName();
+
+  cmd.set_positions_vec(set_pts_);
+}
+
+
+controller::JointPositionController* HeadPanTiltController::getJointPositionControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_position_controllers_[i]->getJointName() == name)
+    {
+      return joint_position_controllers_[i];
+    }
+  }
+    return NULL;
+}
+
+
+int HeadPanTiltController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_position_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+
+void HeadPanTiltController::update(void)
+{
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
+   joint_position_controllers_[i]->setCommand(set_pts_[i]);
+  }
+
+  updateJointControllers();
+}
+
+
+
+void HeadPanTiltController::updateJointControllers(void)
+{
+  for(unsigned int i=0;i<num_joints_;++i)
+    joint_position_controllers_[i]->update();
+}
+
+
+//------ Head controller node --------
+
+ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
+
+HeadPanTiltControllerNode::HeadPanTiltControllerNode()
+  : Controller()
+{
+  std::cout<<"Controller node created"<<endl;
+  c_ = new HeadPanTiltController();
+}
+
+HeadPanTiltControllerNode::~HeadPanTiltControllerNode()
+{
+  delete c_;
+}
+
+void HeadPanTiltControllerNode::update()
+{
+  c_->update();
+}
+
+bool HeadPanTiltControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  std::cout<<"LOADING HEAD PAN TILT CONTROLLER NODE"<<std::endl;
+  ros::node * const node = ros::node::instance();
+  string prefix = config->Attribute("name");
+  std::cout<<"the prefix is "<<prefix<<std::endl;
+  
+  // Parses subcontroller configuration
+  if(c_->initXml(robot, config))
+  {
+    node->advertise_service(prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointSrv, this);
+    node->advertise_service(prefix + "/get_command", &HeadPanTiltControllerNode::getJointCmd, this);
+
+    return true;
+  }
+  return false;
+}
+
+bool HeadPanTiltControllerNode::setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+                                   pr2_mechanism_controllers::SetJointCmd::response &resp)
+{
+  std::vector<double> pos;
+  std::vector<std::string> names;
+
+  req.set_positions_vec(pos);
+  req.set_names_vec(names);
+
+  c_->setJointCmd(pos,names);
+  return true;
+}
+
+bool HeadPanTiltControllerNode::getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
+                    pr2_mechanism_controllers::GetJointCmd::response &resp)
+{
+  pr2_mechanism_controllers::JointCmd cmd;
+  c_->getJointCmd(cmd);
+  resp.command = cmd;
+  return true;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5095
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5095&view=rev
Author:   stuglaser
Date:     2008-10-07 00:12:30 +0000 (Tue, 07 Oct 2008)

Log Message:
-----------
First cut at a caster controller.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-07 00:06:05 UTC (rev 5094)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-07 00:12:30 UTC (rev 5095)
@@ -12,7 +12,8 @@
   src/arm_position_controller.cpp
   src/arm_velocity_controller.cpp
   src/arm_dynamics_controller.cpp
-  src/head_pan_tilt_controller.cpp)
+  src/head_pan_tilt_controller.cpp
+  src/caster_controller.cpp)
 
 rospack_add_executable(testBaseController test/test_base_controller.cpp)
 target_link_libraries(testBaseController pr2_mechanism_controllers)

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h	2008-10-07 00:12:30 UTC (rev 5095)
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Stuart Glaser
+
+ Example config:
+
+ <controller type="CasterController" name="a_caster">
+   <joints caster="caster_joint" wheel_l="wheel_left_joint" wheel_r="wheel_right_joint">
+   <caster_pid p="5.0" i="0.0" d="0.0" iClamp="0.0" />
+   <wheel_pid p="4.0" i="0.0" d="0.0" iClamp="0.0" />
+ </controller>
+ */
+#ifndef CASTER_CONTROLLER_H
+#define CASTER_CONTROLLER_H
+
+#include "mechanism_model/controller.h"
+#include "mechanism_model/robot.h"
+#include "control_toolbox/pid.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "misc_utils/subscription_guard.h"
+#include "std_msgs/Float64.h"
+
+namespace controller {
+
+class CasterController : public Controller
+{
+public:
+  const static double WHEEL_RADIUS;
+  const static double WHEEL_OFFSET;
+
+  CasterController();
+  ~CasterController();
+
+  bool init(mechanism::RobotState *robot_state,
+            const std::string &caster_joint,
+            const std::string &wheel_l_joint, const std::string &wheel_r_joint,
+            const control_toolbox::Pid &caster_pid, const control_toolbox::Pid &wheel_pid);
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  void update();
+
+  double steer_velocity_;
+  double drive_velocity_;
+
+  double getSteerPosition() { return caster_->position_; }
+  double getSteerVelocity() { return caster_->velocity_; }
+
+private:
+  mechanism::JointState *caster_;
+  JointVelocityController caster_vel_, wheel_l_vel_, wheel_r_vel_;
+};
+
+
+/*
+ * Listens on /name/steer_velocity and /name/drive_velocity
+ */
+
+class CasterControllerNode : public Controller
+{
+public:
+  CasterControllerNode();
+  ~CasterControllerNode();
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  void update();
+
+  void setSteerVelocity() {
+    c_.steer_velocity_ = steer_velocity_msg_.data;
+  }
+  void setDriveVelocity() {
+    c_.drive_velocity_ = drive_velocity_msg_.data;
+  }
+
+private:
+  CasterController c_;
+
+  SubscriptionGuard guard_steer_velocity_, guard_drive_velocity_;
+  std_msgs::Float64 steer_velocity_msg_, drive_velocity_msg_;
+};
+
+}
+
+#endif

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp	2008-10-07 00:12:30 UTC (rev 5095)
@@ -0,0 +1,178 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Stuart Glaser
+ */
+
+#include "pr2_mechanism_controllers/caster_controller.h"
+
+namespace controller {
+
+const double CasterController::WHEEL_RADIUS = 0.079;
+const double CasterController::WHEEL_OFFSET = 0.049;
+
+CasterController::CasterController()
+  : steer_velocity_(0), drive_velocity_(0)
+{
+}
+
+CasterController::~CasterController()
+{
+}
+
+bool CasterController::init(
+  mechanism::RobotState *robot,
+  const std::string &caster_joint,
+  const std::string &wheel_l_joint, const std::string &wheel_r_joint,
+  const control_toolbox::Pid &caster_pid, const control_toolbox::Pid &wheel_pid)
+{
+  caster_ = robot->getJointState(caster_joint);
+  if (!caster_)
+  {
+    fprintf(stderr, "Error: Caster joint \"%s\" does not exist\n", caster_joint.c_str());
+    return false;
+  }
+
+  if (!caster_vel_.init(robot, caster_joint, caster_pid))
+    return false;
+  if (!wheel_l_vel_.init(robot, wheel_l_joint, wheel_pid))
+    return false;
+  if (!wheel_r_vel_.init(robot, wheel_r_joint, wheel_pid))
+    return false;
+
+  return true;
+}
+
+bool CasterController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  TiXmlElement *jel = config->FirstChildElement("joints");
+  if (!jel)
+  {
+    fprintf(stderr, "Error: CasterController could not find joints element\n");
+    return false;
+  }
+
+  const char *caster_joint = jel->Attribute("caster");
+  const char *wheel_l_joint = jel->Attribute("wheel_l");
+  const char *wheel_r_joint = jel->Attribute("wheel_r");
+  if (!caster_joint)
+  {
+    fprintf(stderr, "Error: CasterController was not given a caster joint\n");
+    return false;
+  }
+  if (!wheel_l_joint)
+  {
+    fprintf(stderr, "Error: CasterController was not given a wheel_l joint\n");
+    return false;
+  }
+  if (!wheel_r_joint)
+  {
+    fprintf(stderr, "Error: CasterController was not given a wheel_r joint\n");
+    return false;
+  }
+
+  control_toolbox::Pid caster_pid, wheel_pid;
+  TiXmlElement *cpel = config->FirstChildElement("caster_pid");
+  TiXmlElement *wpel = config->FirstChildElement("wheel_pid");
+  if (!cpel)
+  {
+    fprintf(stderr, "Error: CasterController was not given a caster_pid element\n");
+    return false;
+  }
+  if (!caster_pid.initXml(cpel))
+    return false;
+  if (!wpel)
+  {
+    fprintf(stderr, "Error: CasterController was not given a wheel_pid element\n");
+    return false;
+  }
+  if (!wheel_pid.initXml(wpel))
+    return false;
+
+  return init(robot, caster_joint, wheel_l_joint, wheel_r_joint, caster_pid, wheel_pid);
+}
+
+void CasterController::update()
+{
+  caster_vel_.setCommand(steer_velocity_);
+
+  double wd = drive_velocity_ * WHEEL_RADIUS;  // Angular velocity due to driving
+  double ws = (WHEEL_RADIUS / WHEEL_OFFSET) * steer_velocity_;  // Angular velocity due to steering
+  wheel_r_vel_.setCommand(wd + ws);
+  wheel_l_vel_.setCommand(-wd + ws);
+
+  caster_vel_.update();
+  wheel_l_vel_.update();
+  wheel_r_vel_.update();
+}
+
+
+
+ROS_REGISTER_CONTROLLER(CasterControllerNode);
+
+CasterControllerNode::CasterControllerNode()
+{
+}
+
+CasterControllerNode::~CasterControllerNode()
+{
+}
+
+bool CasterControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  ros::node *node = ros::node::instance();
+  assert(node);
+
+  std::string name = config->Attribute("name") ? config->Attribute("name") : "";
+  if (name == "")
+  {
+    fprintf(stderr, "Error: No name given for CasterControllerNode\n");
+    return false;
+  }
+
+  if (!c_.initXml(robot, config))
+    return false;
+
+  node->subscribe(name + "/steer_velocity", steer_velocity_msg_,
+                  &CasterControllerNode::setSteerVelocity, this, 2);
+  guard_steer_velocity_.set(name + "/steer_velocity");
+  node->subscribe(name + "/drive_velocity", drive_velocity_msg_,
+                  &CasterControllerNode::setDriveVelocity, this, 2);
+  guard_drive_velocity_.set(name + "/drive_velocity");
+
+  return true;
+}
+
+void CasterControllerNode::update()
+{
+  c_.update();
+}
+
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5105
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5105&view=rev
Author:   mmwise
Date:     2008-10-07 20:45:19 +0000 (Tue, 07 Oct 2008)

Log Message:
-----------
added coord transform for point tracking

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrackPoint.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-07 18:31:59 UTC (rev 5104)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-07 20:45:19 UTC (rev 5105)
@@ -40,23 +40,20 @@
 #include <mechanism_model/controller.h>
 #include <mechanism_model/joint.h>
 #include <robot_mechanism_controllers/joint_position_controller.h>
-#include <robot_mechanism_controllers/joint_velocity_controller.h>
-#include <robot_mechanism_controllers/joint_effort_controller.h>
 
 // Services
 #include <pr2_mechanism_controllers/SetJointCmd.h>
 #include <pr2_mechanism_controllers/GetJointCmd.h>
+#include <pr2_mechanism_controllers/TrackPoint.h>
 
 #include <pr2_mechanism_controllers/SetJointGains.h>
 #include <pr2_mechanism_controllers/GetJointGains.h>
 
-//Kinematics
-#include <robot_kinematics/robot_kinematics.h>
 
 // Math utils
 #include <math_utils/angles.h>
 
-// #include <libTF/Pose3D.h>
+#include <rosTF/rosTF.h>
 // #include <urdf/URDF.h>
 
 namespace controller
@@ -133,8 +130,9 @@
   void updateJointControllers(void);
 
   int getJointControllerByName(std::string name);
+  
+  
 
-
 };
 
 /** @class HeadPanTiltControllerNode
@@ -185,10 +183,13 @@
      */
     bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
                         pr2_mechanism_controllers::GetJointCmd::response &resp);
-
+    
+    bool trackPoint(pr2_mechanism_controllers::TrackPoint::request &req,                                    
+                      pr2_mechanism_controllers::TrackPoint::response &resp);
   private:
     pr2_mechanism_controllers::JointCmd msg_;   //The message used by the ROS callback
     HeadPanTiltController *c_;
+    rosTFClient TF;
 
 };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-07 18:31:59 UTC (rev 5104)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-07 20:45:19 UTC (rev 5105)
@@ -156,7 +156,7 @@
 ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
 
 HeadPanTiltControllerNode::HeadPanTiltControllerNode()
-  : Controller()
+  : Controller(), TF(*ros::node::instance())
 {
   std::cout<<"Controller node created"<<endl;
   c_ = new HeadPanTiltController();
@@ -184,6 +184,7 @@
   {
     node->advertise_service(prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointSrv, this);
     node->advertise_service(prefix + "/get_command", &HeadPanTiltControllerNode::getJointCmd, this);
+    node->advertise_service(prefix + "/track_point", &HeadPanTiltControllerNode::trackPoint, this);
 
     return true;
   }
@@ -211,3 +212,36 @@
   resp.command = cmd;
   return true;
 }
+
+bool HeadPanTiltControllerNode::trackPoint(pr2_mechanism_controllers::TrackPoint::request &req, pr2_mechanism_controllers::TrackPoint::response &resp)
+{
+  std::vector<double> pos;
+  std::vector<std::string> names;
+
+  libTF::TFPoint point;
+  point.x = req.target.point.x;
+  point.y = req.target.point.y;
+  point.z = req.target.point.z;
+  point.time = req.target.header.stamp.toNSec();
+  point.frame = req.target.header.frame_id;
+  
+  libTF::TFPoint pan_point = TF.transformPoint("head_pan",point);
+  double head_pan_angle= atan2(pan_point.y, pan_point.x); 
+  
+  names.push_back("head_pan_joint");
+  pos.push_back(head_pan_angle);
+  
+  libTF::TFPoint tilt_point = TF.transformPoint("head_tilt",point);
+  double head_tilt_angle= atan2(tilt_point.z, tilt_point.x);
+  
+  names.push_back("head_tilt_joint");
+  pos.push_back(head_tilt_angle);
+  
+     
+  c_->setJointCmd(pos,names);
+  resp.pan_angle = head_pan_angle;
+  resp.tilt_angle = head_tilt_angle;
+  return true;
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrackPoint.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrackPoint.srv	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrackPoint.srv	2008-10-07 20:45:19 UTC (rev 5105)
@@ -0,0 +1,4 @@
+std_msgs/PointStamped target
+---
+float64 pan_angle 
+float64 tilt_angle


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5126
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5126&view=rev
Author:   sachinchitta
Date:     2008-10-07 23:06:27 +0000 (Tue, 07 Oct 2008)

Log Message:
-----------
Modified base controller with acceleration limits

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-07 23:00:05 UTC (rev 5125)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-07 23:06:27 UTC (rev 5126)
@@ -186,8 +186,10 @@
 
     void setCommands();
 
-    NEWMAT::Matrix iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter);
+    libTF::Vector interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT);
 
+   NEWMAT::Matrix iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter);
+
     NEWMAT::Matrix findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type);
 
     int ils_max_iterations_;
@@ -325,11 +327,16 @@
 
     double timeout_;
 
-    double max_x_dot_;
+    libTF::Vector max_vel_; 
 
-    double max_y_dot_;
+    libTF::Vector max_accel_;
 
-    double max_yaw_dot_;
+    double MAX_DT_;
+
+    std::map<std::string, double*> param_map_; /*< map from pointers to the params to string names */
+
+    void addParamToMap(std::string key, double *value);
+
   };
 
   class BaseControllerNode : public Controller

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-10-07 23:00:05 UTC (rev 5125)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_dynamics_controller.cpp	2008-10-07 23:06:27 UTC (rev 5126)
@@ -229,7 +229,7 @@
     }
     pid_torque = pid_controllers_[i].updatePid(error, time - last_time_);
     control_torque_[i] = gravity_torque_[i][2] + pid_torque;
-    fprintf(stderr,"%d:: %f, %f, %f, %f, %f, %f, %f\n",i,actual,command,error,pid_torque,gravity_torque_[i][2],control_torque_[i],time-last_time_);
+//    fprintf(stderr,"%d:: %f, %f, %f, %f, %f, %f, %f\n",i,actual,command,error,pid_torque,gravity_torque_[i][2],control_torque_[i],time-last_time_);
   }
   last_time_ = time;
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-07 23:00:05 UTC (rev 5125)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-07 23:06:27 UTC (rev 5126)
@@ -45,26 +45,38 @@
 
 ROS_REGISTER_CONTROLLER(BaseController)
 
-  BaseController::BaseController() : num_wheels_(0), num_casters_(0)
+BaseController::BaseController() : num_wheels_(0), num_casters_(0)
 {
-  cmd_vel_.x = 0;
-  cmd_vel_.y = 0;
-  cmd_vel_.z = 0;
-  cmd_vel_t_.x = 0;
-  cmd_vel_t_.y = 0;
-  cmd_vel_t_.z = 0;
-  kp_speed_ = KP_SPEED_DEFAULT;
-  base_odom_position_.x = 0;
-  base_odom_position_.y = 0;
-  base_odom_position_.z = 0;
-  base_odom_velocity_.x = 0;
-  base_odom_velocity_.y = 0;
-  base_odom_velocity_.z = 0;
-  ils_weight_type_ = "Gaussian";
-  ils_max_iterations_ = 3;
-  caster_steer_vel_gain_ = 0;
-  timeout_ = 3;
-  pthread_mutex_init(&base_controller_lock_,NULL);
+   MAX_DT_ = 0.01;
+
+   max_accel_.x = 0.1;
+   max_accel_.y = 0.1;
+   max_accel_.z = 0.1;
+
+   cmd_vel_.x = 0;
+   cmd_vel_.y = 0;
+   cmd_vel_.z = 0;
+
+   cmd_vel_t_.x = 0;
+   cmd_vel_t_.y = 0;
+   cmd_vel_t_.z = 0;
+
+   kp_speed_ = KP_SPEED_DEFAULT;
+
+   base_odom_position_.x = 0;
+   base_odom_position_.y = 0;
+   base_odom_position_.z = 0;
+
+   base_odom_velocity_.x = 0;
+   base_odom_velocity_.y = 0;
+   base_odom_velocity_.z = 0;
+
+   ils_weight_type_ = "Gaussian";
+   ils_max_iterations_ = 3;
+   caster_steer_vel_gain_ = 0;
+   timeout_ = 3;
+
+   pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
 BaseController::~BaseController()
@@ -74,201 +86,244 @@
 // Set the joint position command
 void BaseController::setCommand(libTF::Vector cmd_vel)
 {
-  pthread_mutex_lock(&base_controller_lock_);
 
-  cmd_vel_t_.x = clamp(cmd_vel.x,-max_x_dot_, max_x_dot_);
-  cmd_vel_t_.y = clamp(cmd_vel.y,-max_y_dot_, max_y_dot_);
-  cmd_vel_t_.z = clamp(cmd_vel.z,-max_yaw_dot_,max_yaw_dot_);
-  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+   pthread_mutex_lock(&base_controller_lock_);
+   cmd_vel_t_.x = clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+   cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+   cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
+   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 //  std::cout << "command received : " << cmd_vel_t_ << std::endl;
-  pthread_mutex_unlock(&base_controller_lock_);
+   pthread_mutex_unlock(&base_controller_lock_);
 }
 
+libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
+{
+   libTF::Vector result;
+   result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
+   result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
+   result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
+   return result;
+}
+
 libTF::Vector BaseController::getCommand()// Return the current position command
 {
-  libTF::Vector cmd_vel;
-  pthread_mutex_lock(&base_controller_lock_);
-  cmd_vel.x = cmd_vel_t_.x;
-  cmd_vel.y = cmd_vel_t_.y;
-  cmd_vel.z = cmd_vel_t_.z;
-  pthread_mutex_unlock(&base_controller_lock_);
-  return cmd_vel;
+   libTF::Vector cmd_vel;
+   pthread_mutex_lock(&base_controller_lock_);
+   cmd_vel.x = cmd_vel_t_.x;
+   cmd_vel.y = cmd_vel_t_.y;
+   cmd_vel.z = cmd_vel_t_.z;
+   pthread_mutex_unlock(&base_controller_lock_);
+   return cmd_vel;
 }
 
 void BaseController::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot_state)
 {
-  std::vector<JointControlParam>::iterator jcp_iter;
-  robot_desc::URDF::Link *link;
-  std::string joint_name;
+   std::vector<JointControlParam>::iterator jcp_iter;
+   robot_desc::URDF::Link *link;
+   std::string joint_name;
 
-  std::string xml_content;
-  (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+   std::string xml_content;
+   (ros::g_node)->get_param("robotdesc/pr2",xml_content);
 
-  // wait for robotdesc/pr2 on param server
-  while(!urdf_model_.loadString(xml_content.c_str()))
-  {
-    std::cout << "WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar." << std::endl;
-    (ros::g_node)->get_param("robotdesc/pr2",xml_content);
-    usleep(100000);
-  }
+   // wait for robotdesc/pr2 on param server
+   while(!urdf_model_.loadString(xml_content.c_str()))
+   {
+      std::cout << "WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar." << std::endl;
+      (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+      usleep(100000);
+   }
 
-  for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
-  {
-    joint_name = jcp_iter->joint_name;
-    link = urdf_model_.getJointLink(joint_name);
+   for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
+   {
+      joint_name = jcp_iter->joint_name;
+      link = urdf_model_.getJointLink(joint_name);
 
-    BaseParam base_object;
-    base_object.pos_.x = link->xyz[0];
-    base_object.pos_.y = link->xyz[1];
-    base_object.pos_.z = link->xyz[2];
-    base_object.name_ = joint_name;
-    base_object.parent_ = NULL;
-    base_object.joint_state_ = robot_state->getJointState(joint_name);
-    if (base_object.joint_state_==NULL)
-      std::cout << " unsuccessful getting joint state for " << joint_name << std::endl;
+      BaseParam base_object;
+      base_object.pos_.x = link->xyz[0];
+      base_object.pos_.y = link->xyz[1];
+      base_object.pos_.z = link->xyz[2];
+      base_object.name_ = joint_name;
+      base_object.parent_ = NULL;
+      base_object.joint_state_ = robot_state->getJointState(joint_name);
+      if (base_object.joint_state_==NULL)
+         std::cout << " unsuccessful getting joint state for " << joint_name << std::endl;
 
-    base_object.controller_.init(robot_state, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+      base_object.controller_.init(robot_state, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
 
-    if(joint_name.find("caster") != string::npos)
-    {
-      std::cout << " assigning casters " << joint_name << std::endl;
-      base_object.local_id_ = num_casters_;
-      base_casters_.push_back(base_object);
-      steer_angle_actual_.push_back(0);
-      steer_velocity_desired_.push_back(0);
-      num_casters_++;
-      cout << "base_casters" << "::  " << base_object;
-    }
+      if(joint_name.find("caster") != string::npos)
+      {
+         std::cout << " assigning casters " << joint_name << std::endl;
+         base_object.local_id_ = num_casters_;
+         base_casters_.push_back(base_object);
+         steer_angle_actual_.push_back(0);
+         steer_velocity_desired_.push_back(0);
+         num_casters_++;
+         cout << "base_casters" << "::  " << base_object;
+      }
 
-    if(joint_name.find("wheel") != string::npos)
-    {
-      std::cout << " assigning wheels " << joint_name << std::endl;
-      base_object.local_id_ = num_wheels_;
-      if(joint_name.find("_r_") != string::npos)
-        base_object.direction_multiplier_ = 1;
+      if(joint_name.find("wheel") != string::npos)
+      {
+         std::cout << " assigning wheels " << joint_name << std::endl;
+         base_object.local_id_ = num_wheels_;
+         if(joint_name.find("_r_") != string::npos)
+            base_object.direction_multiplier_ = 1;
 
-      base_wheels_.push_back(base_object);
-      wheel_speed_actual_.push_back(0);
-      libTF::Vector *v=new libTF::Vector();
-      base_wheels_position_.push_back(*v);
-      wheel_speed_cmd_.push_back(0);      
-      num_wheels_++;
-    }
-  }
+         base_wheels_.push_back(base_object);
+         wheel_speed_actual_.push_back(0);
+         libTF::Vector *v=new libTF::Vector();
+         base_wheels_position_.push_back(*v);
+         wheel_speed_cmd_.push_back(0);      
+         num_wheels_++;
+      }
+   }
 
-  for(int i =0; i < num_wheels_; i++)
-  {
-    link = urdf_model_.getJointLink(base_wheels_[i].name_);
-    std::string parent_name = link->parent->joint->name;
+   for(int i =0; i < num_wheels_; i++)
+   {
+      link = urdf_model_.getJointLink(base_wheels_[i].name_);
+      std::string parent_name = link->parent->joint->name;
 //    cout << "parent_name from urdf:: " << parent_name << endl;
-    for(int j =0; j < num_casters_; j++)
-    {
-      if(parent_name == base_casters_[j].name_)
+      for(int j =0; j < num_casters_; j++)
       {
+         if(parent_name == base_casters_[j].name_)
+         {
 //        cout <<  "base_casters matched name  ::" << base_casters_[j].name_ << endl;
-        base_wheels_[i].parent_ = &base_casters_[j];
-        break;
+            base_wheels_[i].parent_ = &base_casters_[j];
+            break;
+         }
       }
-    }
 //      cout << "parent assigned name ::" << base_wheels_[i].parent_->name_ << endl << endl;
 //      cout << "base_wheels" << endl << base_wheels_[i];
-  }
+   }
 
-  if(num_wheels_ > 0)
-  {
-    link = urdf_model_.getJointLink(base_wheels_[0].name_);
-    robot_desc::URDF::Link::Geometry::Cylinder *wheel_geom = dynamic_cast<robot_desc::URDF::Link::Geometry::Cylinder*> (link->collision->geometry->shape);
-    wheel_radius_ = wheel_geom->radius;
-  }
-  else
-  {
-    wheel_radius_ = DEFAULT_WHEEL_RADIUS;
-  }
-  robot_state_ = robot_state;
+   if(num_wheels_ > 0)
+   {
+      link = urdf_model_.getJointLink(base_wheels_[0].name_);
+      robot_desc::URDF::Link::Geometry::Cylinder *wheel_geom = dynamic_cast<robot_desc::URDF::Link::Geometry::Cylinder*> (link->collision->geometry->shape);
+      wheel_radius_ = wheel_geom->radius;
+   }
+   else
+   {
+      wheel_radius_ = DEFAULT_WHEEL_RADIUS;
+   }
+   robot_state_ = robot_state;
 
-  last_time_ = robot_state_->hw_->current_time_;
+   last_time_ = robot_state_->hw_->current_time_;
 
-  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 }
 
 bool BaseController::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
-  std::cout << " base controller name: " << config->Attribute("name") << std::endl;
-  TiXmlElement *elt = config->FirstChildElement("controller");
+
+   std::cout << " base controller name: " << config->Attribute("name") << std::endl;
+   TiXmlElement *elt = config->FirstChildElement("controller");
 //  std::cout << " child " << std::endl << *elt << std::endl;
-  std::vector<JointControlParam> jcp_vec;
-  JointControlParam jcp;
-  while (elt){
-    TiXmlElement *jnt = elt->FirstChildElement("joint");
-    //std::cout << "joint snippit" << std::endl << *jnt << std::endl;
+   std::vector<JointControlParam> jcp_vec;
+   JointControlParam jcp;
+   while (elt){
+      TiXmlElement *jnt = elt->FirstChildElement("joint");
+      //std::cout << "joint snippit" << std::endl << *jnt << std::endl;
 
-    // TODO: error check if xml attributes/elements are missing
-    jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
-    jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
-    jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
-    jcp.windup = atof(jnt->FirstChildElement("pid")->Attribute("iClamp"));
-    jcp.control_type = (std::string) elt->Attribute("type");
-    jcp.joint_name = jnt->Attribute("name");
-    jcp_vec.push_back(jcp);
+      // TODO: error check if xml attributes/elements are missing
+      jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
+      jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
+      jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
+      jcp.windup = atof(jnt->FirstChildElement("pid")->Attribute("iClamp"));
+      jcp.control_type = (std::string) elt->Attribute("type");
+      jcp.joint_name = jnt->Attribute("name");
+      jcp_vec.push_back(jcp);
 
-    std::cout << "name:" << jcp.joint_name << std::endl;
-    std::cout << "controller type:" << jcp.control_type << std::endl;
-    std::cout << " sub controller : " << elt->Attribute("name") << std::endl;
+      std::cout << "name:" << jcp.joint_name << std::endl;
+      std::cout << "controller type:" << jcp.control_type << std::endl;
+      std::cout << " sub controller : " << elt->Attribute("name") << std::endl;
 
-    elt = elt->NextSiblingElement("controller");
-  }
+      elt = elt->NextSiblingElement("controller");
+   }
 
-  elt = config->FirstChildElement("map");
-  while(elt)
-  {
-    if(elt->Attribute("name") == std::string("velocity_control"))
-    {
-      TiXmlElement *elt_key = elt->FirstChildElement("elem");
-      while(elt_key)
+   addParamToMap("kp_speed",&kp_speed_);
+   addParamToMap("kp_caster_steer",&caster_steer_vel_gain_);
+   addParamToMap("timeout",&timeout_);
+   addParamToMap("max_x_dot",&(max_vel_.x));
+   addParamToMap("max_y_dot",&(max_vel_.y));
+   addParamToMap("max_yaw_dot",&(max_vel_.z));
+   addParamToMap("max_x_accel",&(max_accel_.x));
+   addParamToMap("max_y_accel",&(max_accel_.y));
+   addParamToMap("max_yaw_accel",&(max_accel_.z));
+
+   elt = config->FirstChildElement("map");
+
+   while(elt)
+   {
+      if(elt->Attribute("name") == std::string("velocity_control"))
       {
-        if(elt_key->Attribute("key") == std::string("kp_speed"))
-        {
-          kp_speed_ = atof(elt_key->GetText());
-        }
-        if(elt_key->Attribute("key") == std::string("kp_caster_steer"))
-        {
-           caster_steer_vel_gain_ = atof(elt_key->GetText());
-        }
-        if(elt_key->Attribute("key") == std::string("timeout"))
-        {
-           timeout_ = atof(elt_key->GetText());
-        }
-        if(elt_key->Attribute("key") == std::string("max_x_dot"))
-        {
-           max_x_dot_ = atof(elt_key->GetText());
-        }
-        if(elt_key->Attribute("key") == std::string("max_y_dot"))
-        {
-           max_y_dot_ = atof(elt_key->GetText());
-        }
-        if(elt_key->Attribute("key") == std::string("max_yaw_dot"))
-        {
-           max_yaw_dot_ = atof(elt_key->GetText());
-        }
-        elt_key = elt_key->NextSiblingElement("elem");
+         TiXmlElement *elt_key = elt->FirstChildElement("elem");
+         while(elt_key)
+         {
+            *(param_map_[elt_key->Attribute("key")]) = atof(elt_key->GetText());
+            
+            if(elt_key->Attribute("key") == std::string("kp_speed"))
+            {
+               kp_speed_ = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("kp_caster_steer"))
+            {
+               caster_steer_vel_gain_ = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("timeout"))
+            {
+               timeout_ = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("max_x_dot"))
+            {
+               max_vel_.x = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("max_y_dot"))
+            {
+               max_vel_.y = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("max_yaw_dot"))
+            {
+               max_vel_.z = atof(elt_key->GetText());
+            }
+
+            if(elt_key->Attribute("key") == std::string("max_x_accel"))
+            {
+               max_accel_.x = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("max_y_accel"))
+            {
+               max_accel_.y = atof(elt_key->GetText());
+            }
+            if(elt_key->Attribute("key") == std::string("max_yaw_accel"))
+            {
+               max_accel_.z = atof(elt_key->GetText());
+            }
+
+            elt_key = elt_key->NextSiblingElement("elem");
+         }
       }
-    }
-    std::cout << "*************************************" << std::endl;
+      std::cout << "*************************************" << std::endl;
 //    std::cout << " sub map : " << elt->Attribute("name") << std::endl;
-    elt = config->NextSiblingElement("map");
-  }
+      elt = config->NextSiblingElement("map");
+   }
 
-  init(jcp_vec,robot_state);
-  return true;
+
+   init(jcp_vec,robot_state);
+   return true;
 }
 
+void BaseController::addParamToMap(std::string key, double *value)
+{
+   param_map_[key] = value;
+}
+
 void BaseController::getJointValues()
 {
-  for(int i=0; i < num_casters_; i++)
-    steer_angle_actual_[i] = base_casters_[i].joint_state_->position_;
+   for(int i=0; i < num_casters_; i++)
+      steer_angle_actual_[i] = base_casters_[i].joint_state_->position_;
 
-  for(int i=0; i < num_wheels_; i++)
-    wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
+   for(int i=0; i < num_wheels_; i++)
+      wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
 
 //   for(int i=0; i < num_casters_; i++)
 //     std::cout << " caster angles " << i << " : " << steer_angle_actual_[i] << std::endl;
@@ -278,52 +333,55 @@
 
 void BaseController::computeWheelPositions()
 {
-  libTF::Vector res1;
-  double steer_angle;
-  for(int i=0; i < num_wheels_; i++)
-  {
-    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+   libTF::Vector res1;
+   double steer_angle;
+   for(int i=0; i < num_wheels_; i++)
+   {
+      steer_angle = base_wheels_[i].parent_->joint_state_->position_;
 //    res1 = rotate2D(base_wheels_[i].pos_,steer_angle);
 //    cout << "init position" << base_wheels_[i].pos_.x << " " << base_wheels_[i].pos_.y << " " << base_wheels_[i].pos_.z << endl;
-    res1 = base_wheels_[i].pos_.rot2D(steer_angle);
+      res1 = base_wheels_[i].pos_.rot2D(steer_angle);
 //    cout << "rotated position" << res1;
-    res1 += base_wheels_[i].parent_->pos_;
+      res1 += base_wheels_[i].parent_->pos_;
 //    cout << "added position" << res1;
-    base_wheels_position_[i] = res1;
-    //cout << "base_wheels_position_(" << i << ")" << base_wheels_position_[i];
-  }
+      base_wheels_position_[i] = res1;
+      //cout << "base_wheels_position_(" << i << ")" << base_wheels_position_[i];
+   }
 //  exit(-1);
 }
 
 void BaseController::update()
 {
-  double current_time = robot_state_->hw_->current_time_;
-
-  if(pthread_mutex_trylock(&base_controller_lock_)==0)
-  {
-    if((robot_state_->hw_->current_time_ - cmd_received_timestamp_) > timeout_)
-    {
+   double current_time = robot_state_->hw_->current_time_;
+   double dT = std::max<double>(current_time - last_time_,MAX_DT_);
+   if(pthread_mutex_trylock(&base_controller_lock_)==0)
+   {
+      if((robot_state_->hw_->current_time_ - cmd_received_timestamp_) > timeout_)
+      {
 //      cout << "BaseController:: timing out" << endl;
-    cmd_vel_.x = 0;
-    cmd_vel_.y = 0;
-    cmd_vel_.z = 0;
-    }
-    else
-    {
+         cmd_vel_.x = 0;
+         cmd_vel_.y = 0;
+         cmd_vel_.z = 0;
+      }
+      else
+      {
 //      cout << "BaseController:: running" << endl;
-    cmd_vel_.x = cmd_vel_t_.x;
-    cmd_vel_.y = cmd_vel_t_.y;
-    cmd_vel_.z = cmd_vel_t_.z;
-    }
-    pthread_mutex_unlock(&base_controller_lock_);
-  }
+
+         cmd_vel_ = interpolateCommand(cmd_vel_,cmd_vel_t_,max_accel_,dT);
+
+//    cmd_vel_.x = cmd_vel_t_.x;
+//    cmd_vel_.y = cmd_vel_t_.y;
+//    cmd_vel_.z = cmd_vel_t_.z;
+      }
+      pthread_mutex_unlock(&base_controller_lock_);
+   }
 //  std::cout << "command received in update : " << cmd_vel_ << std::endl;
 
-  getJointValues();
+   getJointValues();
 
-  computeCommands();
+   computeCommands();
 
-  setCommands();
+   setCommands();
 
 /*  computeWheelPositions();
 
@@ -331,504 +389,504 @@
 
 computeAndSetWheelSpeeds();
 */
-  computeOdometry(current_time);
+   computeOdometry(current_time);
 
-  updateJointControllers();
+   updateJointControllers();
 
-  last_time_ = current_time;
+   last_time_ = current_time;
 }
 
 
 void BaseController::computeCommands()
 {
-  computeWheelPositions();
+   computeWheelPositions();
 
-  computeCasterSteer();
+   computeCasterSteer();
 
-  computeWheelSpeeds();  
+   computeWheelSpeeds();  
 }
 
 void BaseController::setCommands()
 {
-  setCasterSteer();
+   setCasterSteer();
 
-  setWheelSpeeds();
+   setWheelSpeeds();
 }
 
 void BaseController::computeCasterSteer()
 {
-  libTF::Vector result;
+   libTF::Vector result;
 
-  double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
-  double error_steer(0.0),error_steer_m_pi(0.0);
+   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
+   double error_steer(0.0),error_steer_m_pi(0.0);
 
-  for(int i=0; i < num_casters_; i++)
-  {
-    result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
+   for(int i=0; i < num_casters_; i++)
+   {
+      result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
 
-    steer_angle_desired = atan2(result.y,result.x);
-    steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
+      steer_angle_desired = atan2(result.y,result.x);
+      steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
 //    printf("Steering cmd choices: %d, %f, %f\n",i,steer_angle_desired,steer_angle_desired_m_pi);
 
-    error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
-    error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
+      error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+      error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
 
-    if(fabs(error_steer_m_pi) < fabs(error_steer))
-    {
-      error_steer = error_steer_m_pi;
-      steer_angle_desired = steer_angle_desired_m_pi;
-    }
-    steer_velocity_desired_[i] =  -kp_speed_*error_steer;
+      if(fabs(error_steer_m_pi) < fabs(error_steer))
+      {
+         error_steer = error_steer_m_pi;
+         steer_angle_desired = steer_angle_desired_m_pi;
+      }
+      steer_velocity_desired_[i] =  -kp_speed_*error_steer;
 
-    //  printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
-  }
+      //  printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
+   }
 }
 
 void BaseController::setCasterSteer()
 {
-  for(int i=0; i < num_casters_; i++)
-    base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
+   for(int i=0; i < num_casters_; i++)
+      base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
 }
 
 void BaseController::computeWheelSpeeds()
 {
-  libTF::Vector wheel_point_velocity;
-  libTF::Vector wheel_point_velocity_projected;
-  libTF::Vector wheel_caster_steer_component;
-  libTF::Vector caster_2d_velocity;
+   libTF::Vector wheel_point_velocity;
+   libTF::Vector wheel_point_velocity_projected;
+   libTF::Vector wheel_caster_steer_component;
+   libTF::Vector caster_2d_velocity;
 
-  caster_2d_velocity.x = 0;
-  caster_2d_velocity.y = 0;
-  caster_2d_velocity.z = 0;
+   caster_2d_velocity.x = 0;
+   caster_2d_velocity.y = 0;
+   caster_2d_velocity.z = 0;
 
-  double steer_angle_actual = 0;
-  for(int i=0; i < (int) num_wheels_; i++)
-  {
-    caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
-    steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
-    wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
+   double steer_angle_actual = 0;
+   for(int i=0; i < (int) num_wheels_; i++)
+   {
+      caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
+      steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
+      wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
 //    cout << "gain " << caster_steer_vel_gain_ << endl;
 //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
 
-    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
 //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
-    wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+      wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
+      wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
 
 //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd_[i] << ", actual wheel speed" << wheel_speed_actual_[i] << ", caster: " << wheel_caster_steer_component.x/wheel_radius_ << ", caster_steer " << caster_2d_velocity.z << std::endl;
-  }
+   }
 }
 
 void BaseController::setWheelSpeeds()
 {
-  for(int i=0; i < (int) num_wheels_; i++)
-  {
-    base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]);
-  }
+   for(int i=0; i < (int) num_wheels_; i++)
+   {
+      base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]);
+   }
 }
 
 void BaseController::computeAndSetCasterSteer()
 {
-  libTF::Vector result;
-  double steer_angle_desired;
-  double error_steer;
-  for(int i=0; i < num_casters_; i++)
-  {
-    result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
-    steer_angle_desired = atan2(result.y,result.x);
-    //     steer_angle_desired =  modNPiBy2(steer_angle_desired);//Clean steer Angle
+   libTF::Vector result;
+   double steer_angle_desired;
+   double error_steer;
+   for(int i=0; i < num_casters_; i++)
+   {
+      result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
+      steer_angle_desired = atan2(result.y,result.x);
+      //     steer_angle_desired =  modNPiBy2(steer_angle_desired);//Clean steer Angle
 
-    error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
-    steer_velocity_desired_[i] = -kp_speed_*error_steer;
-    //    std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
-    base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
-  }
+      error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+      steer_velocity_desired_[i] = -kp_speed_*error_steer;
+      //    std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
+      base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
+   }
 }
 
 void BaseController::computeAndSetWheelSpeeds()
 {
-  libTF::Vector wheel_point_velocity;
-  libTF::Vector wheel_point_velocity_projected;
-  libTF::Vector wheel_caster_steer_component;
-  libTF::Vector caster_2d_velocity;
+   libTF::Vector wheel_point_velocity;
+   libTF::Vector wheel_point_velocity_projected;
+   libTF::Vector wheel_caster_steer_component;
+   libTF::Vector caster_2d_velocity;
 
-  caster_2d_velocity.x = 0;
-  caster_2d_velocity.y = 0;
-  caster_2d_velocity.z = 0;
+   caster_2d_velocity.x = 0;
+   caster_2d_velocity.y = 0;
+   caster_2d_velocity.z = 0;
 
-  double wheel_speed_cmd = 0;
-  double steer_angle_actual = 0;
-  for(int i=0; i < (int) num_wheels_; i++)
-  {
+   double wheel_speed_cmd = 0;
+   double steer_angle_actual = 0;
+   for(int i=0; i < (int) num_wheels_; i++)
+   {
 
-    caster_2d_velocity.z = steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
-    steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
-    wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
-    //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
-    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-    //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
-    wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-    wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-    //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
-    base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd);
-  }
+      caster_2d_velocity.z = steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
+      steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
+      wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
+      //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
+      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+      //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
+      wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
+      wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+      //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
+      base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd);
+   }
 
 }
 
 void BaseController::updateJointControllers()
 {
-  for(int i=0; i < num_wheels_; i++)
-    base_wheels_[i].controller_.update();
-  for(int i=0; i < num_casters_; i++)
-    base_casters_[i].controller_.update();
+   for(int i=0; i < num_wheels_; i++)
+      base_wheels_[i].controller_.update();
+   for(int i=0; i < num_casters_; i++)
+      base_casters_[i].controller_.update();
 }
 
 void BaseController::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
 {
-  odom_msg_.header.frame_id   = "odom";
-  odom_msg_.header.stamp.sec  = (unsigned long)floor(robot_state_->hw_->current_time_);
-  odom_msg_.header.stamp.nsec = (unsigned long)floor(  1e9 * (  robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
+   odom_msg_.header.frame_id   = "odom";
+   odom_msg_.header.stamp.sec  = (unsigned long)floor(robot_state_->hw_->current_time_);
+   odom_msg_.header.stamp.nsec = (unsigned long)floor(  1e9 * (  robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
 
-  odom_msg_.pos.x  = base_odom_position_.x;
-  odom_msg_.pos.y  = base_odom_position_.y;
-  odom_msg_.pos.th = base_odom_position_.z;
+   odom_msg_.pos.x  = base_odom_position_.x;
+   odom_msg_.pos.y  = base_odom_position_.y;
+   odom_msg_.pos.th = base_odom_position_.z;
 
-  odom_msg_.vel.x  = base_odom_velocity_.x;
-  odom_msg_.vel.y  = base_odom_velocity_.y;
-  odom_msg_.vel.th = base_odom_velocity_.z;
+   odom_msg_.vel.x  = base_odom_velocity_.x;
+   odom_msg_.vel.y  = base_odom_velocity_.y;
+   odom_msg_.vel.th = base_odom_velocity_.z;
 
 //   cout << "Base Odometry: Velocity " << base_odom_velocity_;
 //   cout << "Base Odometry: Position " << base_odom_position_;
 }
 
 ROS_REGISTER_CONTROLLER(BaseControllerNode)
-  BaseControllerNode::BaseControllerNode() : odom_publish_count_(10), odom_publish_counter_(0)
+   BaseControllerNode::BaseControllerNode() : odom_publish_count_(10), odom_publish_counter_(0)
 {
-  c_ = new BaseController();
-  node = ros::node::instance();
+   c_ = new BaseController();
+   node = ros::node::instance();
 }
 
 BaseControllerNode::~BaseControllerNode()
 {
-  node->unadvertise_service(service_prefix + "/set_command");
-  node->unadvertise_service(service_prefix + "/get_actual");
-  node->unadvertise("odom");
-  node->unsubscribe("cmd_vel");
+   node->unadvertise_service(service_prefix + "/set_command");
+   node->unadvertise_service(service_prefix + "/get_actual");
+   node->unadvertise("odom");
+   node->unsubscribe("cmd_vel");
 
-  delete c_;
+   delete c_;
 }
 
 void BaseControllerNode::setPublishCount(int publish_count)
 {
-  odom_publish_count_ = publish_count;
+   odom_publish_count_ = publish_count;
 };
 
 void BaseControllerNode::update()
 {
-  c_->update();
+   c_->update();
 
-  c_->setOdomMessage(odom_msg_);
+   c_->setOdomMessage(odom_msg_);
 
-  if(0)
+   if(0)
 //  if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
-  {
-    (ros::g_node)->publish("odom", odom_msg_);
-    odom_publish_counter_ = 0;
+   {
+      (ros::g_node)->publish("odom", odom_msg_);
+      odom_publish_counter_ = 0;
 
-    // FIXME: should this be in here?
-    /***************************************************************/
-    /*                                                             */
-    /*  frame transforms                                           */
-    /*                                                             */
-    /*  TODO: should we send z, roll, pitch, yaw? seems to confuse */
-    /*        localization                                         */
-    /*                                                             */
-    /***************************************************************/
-    double x=0,y=0,yaw=0,vx,vy,vyaw;
-    this->getOdometry(x,y,yaw,vx,vy,vyaw);
-    this->tfs->sendInverseEuler("odom",
-                                "base",
-                                x, y, 0,
-                                yaw, 0, 0,
-                                odom_msg_.header.stamp);
+      // FIXME: should this be in here?
+      /***************************************************************/
+      /*                                                             */
+      /*  frame transforms                                           */
+      /*                                                             */
+      /*  TODO: should we send z, roll, pitch, yaw? seems to confuse */
+      /*        localization                                         */
+      /*                                                             */
+      /***************************************************************/
+      double x=0,y=0,yaw=0,vx,vy,vyaw;
+      this->getOdometry(x,y,yaw,vx,vy,vyaw);
+      this->tfs->sendInverseEuler("odom",
+                                  "base",
+                                  x, y, 0,
+                                  yaw, 0, 0,
+                                  odom_msg_.header.stamp);
 
-  }
+   }
 
-  odom_publish_counter_++;
+   odom_publish_counter_++;
 
 }
 
 bool BaseControllerNode::setCommand(
-  pr2_mechanism_controllers::SetBaseCommand::request &req,
-  pr2_mechanism_controllers::SetBaseCommand::response &resp)
+   pr2_mechanism_controllers::SetBaseCommand::request &req,
+   pr2_mechanism_controllers::SetBaseCommand::response &resp)
 {
-  libTF::Vector command;
-  command.x = req.x_vel;
-  command.y = req.y_vel;
-  command.z = req.theta_vel;
-  c_->setCommand(command);
-  command = c_->getCommand();
-  resp.x_vel = command.x;
-  resp.y_vel = command.y;
-  resp.theta_vel = command.z;
+   libTF::Vector command;
+   command.x = req.x_vel;
+   command.y = req.y_vel;
+   command.z = req.theta_vel;
+   c_->setCommand(command);
+   command = c_->getCommand();
+   resp.x_vel = command.x;
+   resp.y_vel = command.y;
+   resp.theta_vel = command.z;
 
-  return true;
+   return true;
 }
 
 void BaseControllerNode::setCommand(double vx, double vy, double vw)
 {
-  libTF::Vector command;
-  command.x = vx;
-  command.y = vy;
-  command.z = vw;
-  c_->setCommand(command);
-  //std::cout << "command received : " << vx << "," << vy << "," << vw << std::endl;
+   libTF::Vector command;
+   command.x = vx;
+   command.y = vy;
+   command.z = vw;
+   c_->setCommand(command);
+   //std::cout << "command received : " << vx << "," << vy << "," << vw << std::endl;
 }
 
 
 bool BaseControllerNode::getCommand(
-  pr2_mechanism_controllers::GetBaseCommand::request &req,
-  pr2_mechanism_controllers::GetBaseCommand::response &resp)
+   pr2_mechanism_controllers::GetBaseCommand::request &req,
+   pr2_mechanism_controllers::GetBaseCommand::response &resp)
 {
-  libTF::Vector command;
-  command = c_->getCommand();
-  resp.x_vel = command.x;
-  resp.y_vel = command.y;
-  resp.theta_vel = command.z;
+   libTF::Vector command;
+   command = c_->getCommand();
+   resp.x_vel = command.x;
+   resp.y_vel = command.y;
+   resp.theta_vel = command.z;
 
-  return true;
+   return true;
 }
 
 bool BaseControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
-  service_prefix = config->Attribute("name");
+   service_prefix = config->Attribute("name");
 
-  assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
+   assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
 
-  if(!c_->initXml(robot_state, config))
-    return false;
+   if(!c_->initXml(robot_state, config))
+      return false;
 
-  node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
-  node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
+   node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
 
-  node->advertise<std_msgs::RobotBase2DOdom>("odom",10);
+   node->advertise<std_msgs::RobotBase2DOdom>("odom",10);
 
   // receive messages from 2dnav stack
   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
 
-  // for publishing odometry frame transforms odom
-  this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
+   // for publishing odometry frame transforms odom
+   this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
 
-  return true;
+   return true;
 }
 
 void BaseControllerNode::CmdBaseVelReceived()
 {
-  this->ros_lock_.lock();
-  this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
-  this->ros_lock_.unlock();
+   this->ros_lock_.lock();
+   this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+   this->ros_lock_.unlock();
 }
 
 void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
 {
-  c_->getOdometry(x,y,w,vx,vy,vw);
+   c_->getOdometry(x,y,w,vx,vy,vw);
 }
 
 void BaseController::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
 {
-  x = base_odom_position_.x;
-  y = base_odom_position_.y;
-  w = base_odom_position_.z;
+   x = base_odom_position_.x;
+   y = base_odom_position_.y;
+   w = base_odom_position_.z;
 
-  vx = base_odom_velocity_.x;
-  vy = base_odom_velocity_.y;
-  vw = base_odom_velocity_.z;
+   vx = base_odom_velocity_.x;
+   vy = base_odom_velocity_.y;
+   vw = base_odom_velocity_.z;
 }
 
 
 Vector BaseController::computePointVelocity2D(const Vector& pos, const Vector& vel)
 {
-  Vector result;
+   Vector result;
 
-  result.x = vel.x - pos.y * vel.z;
-  result.y = vel.y + pos.x * vel.z;
-  result.z = 0;
+   result.x = vel.x - pos.y * vel.z;
+   result.y = vel.y + pos.x * vel.z;
+   result.z = 0;
 
-  return result;
+   return result;
 }
 
 void BaseController::computeOdometry(double time)
 {
-  double dt = time-last_time_;
+   double dt = time-last_time_;
 //   libTF::Vector base_odom_delta = rotate2D(base_odom_velocity_*dt,base_odom_position_.z);
 
-  computeBaseVelocity();
+   computeBaseVelocity();
 
-  libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
+   libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
 
-  base_odom_delta.z = base_odom_velocity_.z * dt;
-  base_odom_position_ += base_odom_delta;
+   base_odom_delta.z = base_odom_velocity_.z * dt;
+   base_odom_position_ += base_odom_delta;
 }
 
 void BaseController::computeBaseVelocity()
 {
-  Matrix A(2*num_wheels_,1);
-  Matrix C(2*num_wheels_,3);
-  Matrix D(3,1);
-  double steer_angle;
-  double wheel_speed;
-  libTF::Vector caster_2d_velocity;
-  libTF::Vector wheel_caster_steer_component;
+   Matrix A(2*num_wheels_,1);
+   Matrix C(2*num_wheels_,3);
+   Matrix D(3,1);
+   double steer_angle;
+   double wheel_speed;
+   libTF::Vector caster_2d_velocity;
+   libTF::Vector wheel_caster_steer_component;
 
-  caster_2d_velocity.x = 0;
-  caster_2d_velocity.y = 0;
+   caster_2d_velocity.x = 0;
+   caster_2d_velocity.y = 0;
 
-  for(int i = 0; i < num_wheels_; i++) {
-    caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
-    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_; 
+   for(int i = 0; i < num_wheels_; i++) {
+      caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
+      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+      wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_; 
 
-    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
-    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
-  }
+      steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+      A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
+      A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
+   }
 
-  for(int i = 0; i < num_wheels_; i++) {
-    C.element(i*2, 0)   = 1;
-    C.element(i*2, 1)   = 0;
-    C.element(i*2, 2)   = -base_wheels_position_[i].y;
-    C.element(i*2+1, 0) = 0;
-    C.element(i*2+1, 1) = 1;
-    C.element(i*2+1, 2) =  base_wheels_position_[i].x;
-  }
-  D = pseudoInverse(C)*A;
+   for(int i = 0; i < num_wheels_; i++) {
+      C.element(i*2, 0)   = 1;
+      C.element(i*2, 1)   = 0;
+      C.element(i*2, 2)   = -base_wheels_position_[i].y;
+      C.element(i*2+1, 0) = 0;
+      C.element(i*2+1, 1) = 1;
+      C.element(i*2+1, 2) =  base_wheels_position_[i].x;
+   }
+   D = pseudoInverse(C)*A;
 //  D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
-  base_odom_velocity_.x = (double)D.element(0,0);
-  base_odom_velocity_.y = (double)D.element(1,0);
-  base_odom_velocity_.z = (double)D.element(2,0);
+   base_odom_velocity_.x = (double)D.element(0,0);
+   base_odom_velocity_.y = (double)D.element(1,0);
+   base_odom_velocity_.z = (double)D.element(2,0);
 
 //  cout << "Base odom velocity " << base_odom_velocity_ << endl;
 }
 
 Matrix BaseController::pseudoInverse(const Matrix M)
 {
-  Matrix result;
-  //int rows = this->rows();
-  //int cols = this->columns();
-  // calculate SVD decomposition
-  Matrix U,V;
-  DiagonalMatrix D;
-  NEWMAT::SVD(M,D,U,V, true, true);
-  Matrix Dinv = D.i();
-  result = V * Dinv * U.t();
-  return result;
+   Matrix result;
+   //int rows = this->rows();
+   //int cols = this->columns();
+   // calculate SVD decomposition
+   Matrix U,V;
+   DiagonalMatrix D;
+   NEWMAT::SVD(M,D,U,V, true, true);
+   Matrix Dinv = D.i();
+   result = V * Dinv * U.t();
+   return result;
 }
 
 std::ostream & controller::operator<<(std::ostream& mystream, const controller::BaseParam &bp)
 {
-  mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
-  return mystream;
+   mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
+   return mystream;
 };
 
 NEWMAT::Matrix BaseController::iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter)
 {
-  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
-  NEWMAT::Matrix weight_matrix(2*num_wheels_,2*num_wheels_);
-  NEWMAT::Matrix a_fit(2*num_wheels_,3);
-  NEWMAT::Matrix b_fit(2*num_wheels_,1);
-  NEWMAT::Matrix residual(2*num_wheels_,1);
-  NEWMAT::Matrix x_fit(3,1);
+   NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+   NEWMAT::Matrix weight_matrix(2*num_wheels_,2*num_wheels_);
+   NEWMAT::Matrix a_fit(2*num_wheels_,3);
+   NEWMAT::Matrix b_fit(2*num_wheels_,1);
+   NEWMAT::Matrix residual(2*num_wheels_,1);
+   NEWMAT::Matrix x_fit(3,1);
 
-  weight_matrix = ident_matrix;
+   weight_matrix = ident_matrix;
 
 //  cout << "num_wheels: " << num_wheels_ << endl;
-  for(int i=0; i < max_iter; i++)
-  {
-    a_fit = weight_matrix * A;
-    b_fit = weight_matrix * b;
-    x_fit = pseudoInverse(a_fit)*b_fit;
-    residual = b - A * x_fit;
+   for(int i=0; i < max_iter; i++)
+   {
+      a_fit = weight_matrix * A;
+      b_fit = weight_matrix * b;
+      x_fit = pseudoInverse(a_fit)*b_fit;
+      residual = b - A * x_fit;
 
 //    cout << "Residual::" << residual << endl;
 
 
-    for(int j=1; j <= num_wheels_; j++)
-    {
-      int fw = 2*j-1;
-      int sw = fw+1;
-      if(fabs(residual(fw,1)) > fabs(residual(sw,1)))
+      for(int j=1; j <= num_wheels_; j++)
       {
-        residual(fw,1) = fabs(residual(fw,1));
-        residual(sw,1) = residual(fw,1);
+         int fw = 2*j-1;
+         int sw = fw+1;
+         if(fabs(residual(fw,1)) > fabs(residual(sw,1)))
+         {
+            residual(fw,1) = fabs(residual(fw,1));
+            residual(sw,1) = residual(fw,1);
+         }
+         else
+         {
+            residual(fw,1) = fabs(residual(sw,1));
+            residual(sw,1) = residual(fw,1);
+         }
       }
-      else
-      {
-        residual(fw,1) = fabs(residual(sw,1));
-        residual(sw,1) = residual(fw,1);
-      }
-    }
 
 //    cout << "afit::" << a_fit << endl << endl;
 //    cout << "bfit::" << b_fit << endl << endl;
 //    cout << "xfit::" << endl << x_fit << endl;
 //    cout << "Residual::" << residual << endl;
 
-    weight_matrix = findWeightMatrix(residual,weight_type);
+      weight_matrix = findWeightMatrix(residual,weight_type);
 
 //    cout << "weight_matrix" << endl << weight_matrix  << endl << endl;
-  }
-  return x_fit;
+   }
+   return x_fit;
 };
 
 NEWMAT::Matrix BaseController::findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type)
 {
-  NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
-  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+   NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
+   NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
 
-  w_fit = ident_matrix;
-  double epsilon = 0;
-  double g_sigma = 0.1;
+   w_fit = ident_matrix;
+   double epsilon = 0;
+   double g_sigma = 0.1;
 
-  if(weight_type == std::string("BubeLagan"))
-  {
-    for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
-    {
-      if(fabs(residual(i,1) > epsilon))
-        epsilon = fabs(residual(i,1));
-    }
-    epsilon = epsilon/100.0;
-  }
-  for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
-  {
-    if(weight_type == std::string("L1norm"))
-    {
-      w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
-      cout << residual(i,1) << endl;
-    }
-    else if(weight_type == std::string("fair"))
-    {
-      w_fit(i,i) = 1.0/(1 + fabs(residual(i,1)));
-    }
-    else if(weight_type == std::string("Cauchy"))
-    {
-      w_fit(i,i) = 1.0/(1 + residual(i,1)*residual(i,1));
-    }
-    else if(weight_type == std::string("BubeLangan"))
-    {
-      w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
-      cout << residual(i,1) << endl;
-    }
-    else if(weight_type == std::string("Gaussian"))
-    {
-      w_fit(i,i) = sqrt(exp(-pow(residual(i,1),2)/(2*g_sigma*g_sigma)));
-    }
-    else // default to fair
-    {
-      cout << "Default" << endl;
-      w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
-    }
-  }
-  return w_fit;
+   if(weight_type == std::string("BubeLagan"))
+   {
+      for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+      {
+         if(fabs(residual(i,1) > epsilon))
+            epsilon = fabs(residual(i,1));
+      }
+      epsilon = epsilon/100.0;
+   }
+   for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+   {
+      if(weight_type == std::string("L1norm"))
+      {
+         w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
+         cout << residual(i,1) << endl;
+      }
+      else if(weight_type == std::string("fair"))
+      {
+         w_fit(i,i) = 1.0/(1 + fabs(residual(i,1)));
+      }
+      else if(weight_type == std::string("Cauchy"))
+      {
+         w_fit(i,i) = 1.0/(1 + residual(i,1)*residual(i,1));
+      }
+      else if(weight_type == std::string("BubeLangan"))
+      {
+         w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
+         cout << residual(i,1) << endl;
+      }
+      else if(weight_type == std::string("Gaussian"))
+      {
+         w_fit(i,i) = sqrt(exp(-pow(residual(i,1),2)/(2*g_sigma*g_sigma)));
+      }
+      else // default to fair
+      {
+         cout << "Default" << endl;
+         w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
+      }
+   }
+   return w_fit;
 };
 
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5181
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5181&view=rev
Author:   sachinchitta
Date:     2008-10-08 23:45:29 +0000 (Wed, 08 Oct 2008)

Log Message:
-----------
Added real time publisher to the base controller, test_odom tests it against the simulator

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-08 23:34:23 UTC (rev 5180)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-08 23:45:29 UTC (rev 5181)
@@ -23,3 +23,6 @@
 
 rospack_add_executable(testArmDynamicsController test/test_arm_dynamics_controller.cpp)
 target_link_libraries(testRunBaseController pr2_mechanism_controllers)
+
+rospack_add_executable(testOdom test/test_odom.cpp)
+target_link_libraries(testOdom pr2_mechanism_controllers)

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-08 23:34:23 UTC (rev 5180)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-08 23:45:29 UTC (rev 5181)
@@ -55,6 +55,8 @@
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/BaseVel.h>
 
+#include <misc_utils/realtime_publisher.h>
+
 #include <rosTF/rosTF.h>
 
 #include <pthread.h>
@@ -196,6 +198,11 @@
 
     std::string ils_weight_type_;
 
+    /*!
+     * \brief Robot representation
+     */
+    mechanism::RobotState* robot_state_;
+
     private:
 
     /*!
@@ -214,14 +221,7 @@
      */
     double kp_speed_;
 
-
     /*!
-     * \brief Robot representation
-     */
-    mechanism::RobotState* robot_state_;
-
-
-    /*!
      * \brief compute 2D velocity of a point on a rigid body given a 2D input velocity
      * \param pos - Vector (see libTF for more information)
      * \param vel - Vector, vel.x and vel.y represent translational velocities in X and Y directions, vel.z represents rotational(angular velocity)
@@ -403,7 +403,15 @@
      */
     int odom_publish_count_; //FIXME: use time rather than count
 
+    double last_time_message_sent_ ;
 
+    double odom_publish_delta_t_;
+
+    double odom_publish_rate_;
+           
+    misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
+
+
     int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
   };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-08 23:34:23 UTC (rev 5180)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-08 23:45:29 UTC (rev 5181)
@@ -4,6 +4,7 @@
   </description>
   <author>Sachin Chita, John Hsu, David Li, Jimmy Sastra, Melonee Wise</author>
   <license>BSD</license>
+  <depend package="roscpp" />
   <depend package="mechanism_model" />
   <depend package="wg_robot_description_parser" />
   <depend package="stl_utils" />

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-08 23:34:23 UTC (rev 5180)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-08 23:45:29 UTC (rev 5181)
@@ -35,7 +35,9 @@
 #include <pr2_mechanism_controllers/base_controller.h>
 #include <math_utils/angles.h>
 #include <math_utils/math_utils.h>
+#include "ros/node.h"
 
+using namespace ros;
 using namespace std;
 using namespace controller;
 using namespace control_toolbox;
@@ -269,8 +271,8 @@
          while(elt_key)
          {
             *(param_map_[elt_key->Attribute("key")]) = atof(elt_key->GetText());
-            
-            if(elt_key->Attribute("key") == std::string("kp_speed"))
+                        
+/*            if(elt_key->Attribute("key") == std::string("kp_speed"))
             {
                kp_speed_ = atof(elt_key->GetText());
             }
@@ -307,7 +309,7 @@
             {
                max_accel_.z = atof(elt_key->GetText());
             }
-
+*/
             elt_key = elt_key->NextSiblingElement("elem");
          }
       }
@@ -316,6 +318,15 @@
       elt = config->NextSiblingElement("map");
    }
 
+   cout << "kp_speed  " << kp_speed_ << endl;
+   cout << "kp_caster_steer  " << caster_steer_vel_gain_ << endl;
+   cout << "timeout  " << timeout_ << endl;
+   cout << "max_x_dot  " << (max_vel_.x) << endl;
+   cout << "max_y_dot  " << (max_vel_.y) << endl;
+   cout << "max_yaw_dot  " << (max_vel_.z) << endl;
+   cout << "max_x_accel  " << (max_accel_.x) << endl;
+   cout << "max_y_accel  " << (max_accel_.y) << endl;
+   cout << "max_yaw_accel  " << (max_accel_.z) << endl;
 
    init(jcp_vec,robot_state);
    return true;
@@ -365,7 +376,7 @@
    double dT = std::min<double>(current_time - last_time_,MAX_DT_);
    if(pthread_mutex_trylock(&base_controller_lock_)==0)
    {
-      if((robot_state_->hw_->current_time_ - cmd_received_timestamp_) > timeout_)
+      if((current_time - cmd_received_timestamp_) > timeout_)
       {
 //      cout << "BaseController:: timing out" << endl;
          cmd_vel_.x = 0;
@@ -573,15 +584,22 @@
 {
    c_ = new BaseController();
    node = ros::node::instance();
+   last_time_message_sent_ = 0.0;
+   odom_publish_rate_ = 100.0;
+   odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+   publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
 {
    node->unadvertise_service(service_prefix + "/set_command");
    node->unadvertise_service(service_prefix + "/get_actual");
-   node->unadvertise("odom");
+//   node->unadvertise("odom");
    node->unsubscribe("cmd_vel");
 
+   publisher_->stop();
+   delete publisher_;
+
    delete c_;
 }
 
@@ -592,10 +610,21 @@
 
 void BaseControllerNode::update()
 {
+   double time = c_->robot_state_->hw_->current_time_;
+
    c_->update();
-
    c_->setOdomMessage(odom_msg_);
 
+  if (time-last_time_message_sent_ >= odom_publish_delta_t_) // send odom message
+  {
+    if (publisher_->trylock())
+    {
+      c_->setOdomMessage(publisher_->msg_);
+      publisher_->unlockAndPublish() ;
+      last_time_message_sent_ = time;
+    }
+  }
+
    if(0)
 //  if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
    {
@@ -678,14 +707,23 @@
    node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
    node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
 
-   node->advertise<std_msgs::RobotBase2DOdom>("odom",10);
+//   node->advertise<std_msgs::RobotBase2DOdom>("odom",10);
 
   // receive messages from 2dnav stack
   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
 
    // for publishing odometry frame transforms odom
-   this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
+//   this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
 
+  if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete publisher_ ;
+  publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+
+  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,1000);
+
+  if(odom_publish_rate_ > 1e-5)
+     odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+
    return true;
 }
 

Added: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2008-10-08 23:45:29 UTC (rev 5181)
@@ -0,0 +1,199 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <libTF/libTF.h>
+#include <ros/node.h>
+#include <std_msgs/TransformWithRateStamped.h>
+#include <std_msgs/BaseVel.h>
+#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/Quaternion.h>
+#include <iostream>
+#include <fstream>
+
+static int done = 0;
+
+void finalize(int donecare)
+{
+  done = 1;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Return the rotation in Euler angles
+libTF::Vector GetAsEuler(std_msgs::Quaternion quat)
+{
+  libTF::Vector vec;
+
+  double squ;
+  double sqx;
+  double sqy;
+  double sqz;
+
+//  this->Normalize();
+
+  squ = quat.w * quat.w;
+  sqx = quat.x * quat.x;
+  sqy = quat.y * quat.y;
+  sqz = quat.z * quat.z;
+
+  // Roll
+  vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz);
+
+  // Pitch
+  vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y));
+
+  // Yaw
+  vec.z = atan2(2 * (quat.x*quat.y + quat.w*quat.z), squ + sqx - sqy - sqz);
+
+  return vec;
+}
+
+
+class test_run_base
+{
+  public:
+
+    test_run_base():subscriber_connected(0){}; 
+
+    ~test_run_base() {}
+
+    std_msgs::TransformWithRateStamped ground_truth;
+
+    std_msgs::RobotBase2DOdom odom;
+
+    int subscriber_connected;
+
+    void subscriber_connect()
+    {
+      subscriber_connected = 1;
+    }
+
+    void odomMsgReceived()
+    {
+//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
+    };
+
+    void groundTruthMsgReceived()
+    {
+//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
+    };
+};
+
+int main( int argc, char** argv )
+{
+
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("test_run_base_controller"); 
+
+
+  // receive messages from 2dnav stack
+  std_msgs::TransformWithRateStamped ground_truth;
+
+  test_run_base tb;
+
+  node->subscribe("odom",tb.odom,&test_run_base::groundTruthMsgReceived,&tb,10);
+
+  signal(SIGINT,  finalize);
+  signal(SIGQUIT, finalize);
+  signal(SIGTERM, finalize);
+
+
+  /*********** Start moving the robot ************/
+  std_msgs::BaseVel cmd;
+  cmd.vx = 0;
+  cmd.vy = 0;
+  cmd.vw = 0;
+
+  double run_time = 0;
+  bool run_time_set = false;
+  int file_num = 0;
+
+  if(argc >= 2)
+    cmd.vx = atof(argv[1]);
+
+  if(argc >= 3)
+    cmd.vy = atof(argv[2]);
+
+  if(argc >= 4)
+    cmd.vw = atof(argv[3]);
+
+  if(argc >=5)
+  { 
+     run_time = atof(argv[4]);
+     run_time_set = true;
+  }
+
+  if(argc ==6)
+  {
+     file_num = atoi(argv[5]);
+  }
+
+  node->advertise<std_msgs::BaseVel>("cmd_vel",10);
+  sleep(1);
+  node->publish("cmd_vel",cmd);
+  sleep(1);
+
+  libTF::Vector ang_rates;
+  ros::Time start_time = ros::Time::now();
+  ros::Duration sleep_time(0.01);
+
+  std::ofstream odom_log_file;
+  odom_log_file.open("odom_log.txt");
+
+
+  while(!done)
+  {
+     ros::Duration delta_time = ros::Time::now() - start_time;
+
+     if(run_time_set && delta_time.toSec() > run_time)
+        break;
+    //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
+
+//     odom_log_file << 
+
+//    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+     //   cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+     cout << delta_time.toSec() << "  " << run_time << endl;
+    node->publish("cmd_vel",cmd);
+    sleep_time.sleep();
+  }
+
+  node->unsubscribe("odom");
+  node->unadvertise("cmd_vel");
+
+  odom_log_file.close();
+
+  ros::fini();
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-08 23:34:23 UTC (rev 5180)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-08 23:45:29 UTC (rev 5181)
@@ -136,28 +136,44 @@
   cmd.vy = 0;
   cmd.vw = 0;
 
+  double run_time = 0;
+  bool run_time_set = false;
+
   if(argc >= 2)
     cmd.vx = atof(argv[1]);
 
   if(argc >= 3)
     cmd.vy = atof(argv[2]);
 
-  if(argc == 4)
+  if(argc >= 4)
     cmd.vw = atof(argv[3]);
 
+  if(argc ==5)
+  { 
+     run_time = atof(argv[4]);
+     run_time_set = true;
+  }
   node->advertise<std_msgs::BaseVel>("cmd_vel",10);
   sleep(1);
   node->publish("cmd_vel",cmd);
   sleep(1);
 
   libTF::Vector ang_rates;
-
+  ros::Time start_time = ros::Time::now();
+  ros::Duration sleep_time(0.01);
   while(!done)
   {
+     ros::Duration delta_time = ros::Time::now() - start_time;
+
+     if(run_time_set && delta_time.toSec() > run_time)
+        break;
     //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
-    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
-    cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+//    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+     //   cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+     cout << delta_time.toSec() << "  " << run_time << endl;
     node->publish("cmd_vel",cmd);
-    sleep(1);
+    sleep_time.sleep();
   }
+
+  ros::fini();
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5192
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5192&view=rev
Author:   jleibs
Date:     2008-10-09 02:04:12 +0000 (Thu, 09 Oct 2008)

Log Message:
-----------
final hed controller code

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-09 02:03:34 UTC (rev 5191)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-09 02:04:12 UTC (rev 5192)
@@ -116,6 +116,7 @@
 
   unsigned int num_joints_;
   std::vector<JointPositionController *> joint_position_controllers_;  
+  int getJointControllerByName(std::string name);
 
 private:
 
@@ -130,9 +131,9 @@
 
   void updateJointControllers(void);
 
-  int getJointControllerByName(std::string name);
   
   
+  
 
 };
 
@@ -186,10 +187,15 @@
                         pr2_mechanism_controllers::GetJointCmd::response &resp);
     
     bool trackPoint(pr2_mechanism_controllers::TrackPoint::request &req,                                    
-                      pr2_mechanism_controllers::TrackPoint::response &resp);
+                     pr2_mechanism_controllers::TrackPoint::response &resp);
   private:
     pr2_mechanism_controllers::JointCmd msg_;   //The message used by the ROS callback
     HeadPanTiltController *c_;
+    std::string service_prefix;
+    /*
+     * \brief pointer to ros node
+     */
+    ros::node *node;
     rosTFClient TF;
 
 };

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-09 02:03:34 UTC (rev 5191)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-09 02:04:12 UTC (rev 5192)
@@ -157,14 +157,19 @@
 ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
 
 HeadPanTiltControllerNode::HeadPanTiltControllerNode()
-  : Controller(), TF(*ros::node::instance())
+  : Controller(), TF(*ros::node::instance(),false)
 {
+  c_ = new HeadPanTiltController();
+  node = ros::node::instance();  
   std::cout<<"Controller node created"<<endl;
-  c_ = new HeadPanTiltController();
+  
 }
 
 HeadPanTiltControllerNode::~HeadPanTiltControllerNode()
 {
+  node->unadvertise_service(service_prefix + "/set_command_array");
+  node->unadvertise_service(service_prefix + "/get_command");
+
   delete c_;
 }
 
@@ -176,16 +181,15 @@
 bool HeadPanTiltControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
 {
   std::cout<<"LOADING HEAD PAN TILT CONTROLLER NODE"<<std::endl;
-  ros::node * const node = ros::node::instance();
-  string prefix = config->Attribute("name");
-  std::cout<<"the prefix is "<<prefix<<std::endl;
+  service_prefix = config->Attribute("name");
+  std::cout<<"the prefix is "<<service_prefix<<std::endl;
   
   // Parses subcontroller configuration
   if(c_->initXml(robot, config))
   {
-    node->advertise_service(prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointSrv, this);
-    node->advertise_service(prefix + "/get_command", &HeadPanTiltControllerNode::getJointCmd, this);
-    node->advertise_service(prefix + "/track_point", &HeadPanTiltControllerNode::trackPoint, this);
+    node->advertise_service(service_prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointSrv, this);
+    node->advertise_service(service_prefix + "/get_command", &HeadPanTiltControllerNode::getJointCmd, this);
+    node->advertise_service(service_prefix + "/track_point", &HeadPanTiltControllerNode::trackPoint, this);
 
     return true;
   }
@@ -203,8 +207,8 @@
 
   c_->setJointCmd(pos,names);
   c_->getJointCmd(cmds);
-resp.set_positions_vec(pos);
-resp.set_names_vec(cmds.names);
+  resp.set_positions_vec(pos);
+  resp.set_names_vec(cmds.names);
   
   return true;
 }
@@ -231,22 +235,29 @@
   point.frame = req.target.header.frame_id;
   
   libTF::TFPoint pan_point = TF.transformPoint("head_pan",point);
-  double head_pan_angle= atan2(pan_point.y, pan_point.x); 
+  int id = c_->getJointControllerByName("head_pan_joint");
+  assert(id>=0);
+  double mes_pan_angle = c_->joint_position_controllers_[id]->getMeasuredPosition();
+  double head_pan_angle= mes_pan_angle + atan2(pan_point.y, pan_point.x); 
   
   names.push_back("head_pan_joint");
   pos.push_back(head_pan_angle);
   
   libTF::TFPoint tilt_point = TF.transformPoint("head_tilt",point);
-  double head_tilt_angle= atan2(tilt_point.z, tilt_point.x);
+  id = c_->getJointControllerByName("head_tilt_joint");
+  assert(id>=0);
+  double mes_tilt_angle= c_->joint_position_controllers_[id]->getMeasuredPosition();
+  double head_tilt_angle= mes_tilt_angle + atan2(-tilt_point.z, tilt_point.x);
   
   names.push_back("head_tilt_joint");
   pos.push_back(head_tilt_angle);
   
      
   c_->setJointCmd(pos,names);
-  resp.pan_angle = head_pan_angle;
-  resp.tilt_angle = head_tilt_angle;
+  resp.pan_angle = mes_pan_angle;//head_pan_angle;
+  resp.tilt_angle = mes_tilt_angle;//head_tilt_angle;
   return true;
 }
 
 
+


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5193
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5193&view=rev
Author:   sachinchitta
Date:     2008-10-09 04:08:32 +0000 (Thu, 09 Oct 2008)

Log Message:
-----------
Modified method for sending out commands

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-09 02:04:12 UTC (rev 5192)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-09 04:08:32 UTC (rev 5193)
@@ -205,6 +205,8 @@
 
     private:
 
+    bool new_cmd_available_;
+
     /*!
      * \brief number of wheels
      */
@@ -411,7 +413,6 @@
            
     misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
 
-
     int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
   };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-09 02:04:12 UTC (rev 5192)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-09 04:08:32 UTC (rev 5193)
@@ -78,6 +78,8 @@
    caster_steer_vel_gain_ = 0;
    timeout_ = 0.1;
 
+   new_cmd_available_ = false;
+
    pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
@@ -95,6 +97,7 @@
    cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
    cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 //  std::cout << "command received : " << cmd_vel_t_ << std::endl;
+   new_cmd_available_ = true;
    pthread_mutex_unlock(&base_controller_lock_);
 }
 
@@ -374,28 +377,23 @@
 {
    double current_time = robot_state_->hw_->current_time_;
    double dT = std::min<double>(current_time - last_time_,MAX_DT_);
-   if(pthread_mutex_trylock(&base_controller_lock_)==0)
+
+   if(new_cmd_available_)
    {
-      if((current_time - cmd_received_timestamp_) > timeout_)
+      if(pthread_mutex_trylock(&base_controller_lock_)==0)
       {
-//      cout << "BaseController:: timing out" << endl;
-         cmd_vel_.x = 0;
-         cmd_vel_.y = 0;
-         cmd_vel_.z = 0;
-      }
-      else
-      {
-//      cout << "BaseController:: running" << endl;
-
          cmd_vel_ = interpolateCommand(cmd_vel_,cmd_vel_t_,max_accel_,dT);
-
-//    cmd_vel_.x = cmd_vel_t_.x;
-//    cmd_vel_.y = cmd_vel_t_.y;
-//    cmd_vel_.z = cmd_vel_t_.z;
+         new_cmd_available_ = false;
+         pthread_mutex_unlock(&base_controller_lock_);
       }
-      pthread_mutex_unlock(&base_controller_lock_);
-   }
 //  std::cout << "command received in update : " << cmd_vel_ << std::endl;
+   }
+   if((current_time - cmd_received_timestamp_) > timeout_)
+   {
+      cmd_vel_.x = 0;
+      cmd_vel_.y = 0;
+      cmd_vel_.z = 0;
+   }
 
    getJointValues();
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5195
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5195&view=rev
Author:   sachinchitta
Date:     2008-10-09 06:50:23 +0000 (Thu, 09 Oct 2008)

Log Message:
-----------
Put back couts in test_base controller to print ground truth and odometry

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-09 05:30:55 UTC (rev 5194)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-09 06:50:23 UTC (rev 5195)
@@ -248,6 +248,7 @@
      */
     libTF::Vector cmd_vel_t_;
 
+    libTF::Vector desired_vel_;
 
     /*!
      * \brief Position of the robot computed by odometry.

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-09 05:30:55 UTC (rev 5194)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-09 06:50:23 UTC (rev 5195)
@@ -59,6 +59,10 @@
    cmd_vel_.y = 0;
    cmd_vel_.z = 0;
 
+   desired_vel_.x = 0;
+   desired_vel_.y = 0;
+   desired_vel_.z = 0;
+
    cmd_vel_t_.x = 0;
    cmd_vel_t_.y = 0;
    cmd_vel_t_.z = 0;
@@ -123,9 +127,9 @@
 {
    libTF::Vector cmd_vel;
    pthread_mutex_lock(&base_controller_lock_);
-   cmd_vel.x = cmd_vel_t_.x;
-   cmd_vel.y = cmd_vel_t_.y;
-   cmd_vel.z = cmd_vel_t_.z;
+   cmd_vel.x = cmd_vel_.x;
+   cmd_vel.y = cmd_vel_.y;
+   cmd_vel.z = cmd_vel_.z;
    pthread_mutex_unlock(&base_controller_lock_);
    return cmd_vel;
 }
@@ -382,19 +386,25 @@
    {
       if(pthread_mutex_trylock(&base_controller_lock_)==0)
       {
-         cmd_vel_ = interpolateCommand(cmd_vel_,cmd_vel_t_,max_accel_,dT);
+         desired_vel_.x = cmd_vel_t_.x;
+         desired_vel_.y = cmd_vel_t_.y;
+         desired_vel_.z = cmd_vel_t_.z;
          new_cmd_available_ = false;
          pthread_mutex_unlock(&base_controller_lock_);
       }
 //  std::cout << "command received in update : " << cmd_vel_ << std::endl;
    }
+
    if((current_time - cmd_received_timestamp_) > timeout_)
    {
       cmd_vel_.x = 0;
       cmd_vel_.y = 0;
       cmd_vel_.z = 0;
    }
+   else
+      cmd_vel_ = interpolateCommand(cmd_vel_,desired_vel_,max_accel_,dT);
 
+
    getJointValues();
 
    computeCommands();

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-09 05:30:55 UTC (rev 5194)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-09 06:50:23 UTC (rev 5195)
@@ -160,7 +160,7 @@
 
   libTF::Vector ang_rates;
   ros::Time start_time = ros::Time::now();
-  ros::Duration sleep_time(0.01);
+  ros::Duration sleep_time(0.1);
   while(!done)
   {
      ros::Duration delta_time = ros::Time::now() - start_time;
@@ -168,9 +168,9 @@
      if(run_time_set && delta_time.toSec() > run_time)
         break;
     //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
-//    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
-     //   cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
-     cout << delta_time.toSec() << "  " << run_time << endl;
+    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+    cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+    //    cout << delta_time.toSec() << "  " << run_time << endl;
     node->publish("cmd_vel",cmd);
     sleep_time.sleep();
   }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5219
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5219&view=rev
Author:   sachinchitta
Date:     2008-10-10 03:56:04 +0000 (Fri, 10 Oct 2008)

Log Message:
-----------
Modified base_controller with real time publishing of base to odom transform

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-10 03:32:46 UTC (rev 5218)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-10 03:56:04 UTC (rev 5219)
@@ -58,6 +58,7 @@
 #include <misc_utils/realtime_publisher.h>
 
 #include <rosTF/rosTF.h>
+#include <rosTF/TransformEuler.h>
 
 #include <pthread.h>
 
@@ -414,6 +415,8 @@
            
     misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
 
+    misc_utils::RealtimePublisher <rosTF::TransformArray>* transform_publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
+
     int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
   };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-10 03:32:46 UTC (rev 5218)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-10 03:56:04 UTC (rev 5219)
@@ -575,6 +575,7 @@
    odom_publish_rate_ = 100.0;
    odom_publish_delta_t_ = 1.0/odom_publish_rate_;
    publisher_ = NULL;
+   transform_publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
@@ -586,7 +587,7 @@
 
    publisher_->stop();
    delete publisher_;
-
+   delete transform_publisher_;
    delete c_;
 }
 
@@ -611,6 +612,23 @@
       publisher_->unlockAndPublish() ;
       last_time_message_sent_ = time;
     }
+
+    if (transform_publisher_->trylock())
+      {
+      double x=0,y=0,yaw=0,vx,vy,vyaw;
+      this->getOdometry(x,y,yaw,vx,vy,vyaw);
+      rosTF::TransformEuler &out = transform_publisher_->msg_.eulers[0];
+        out.header.stamp.from_double(time);
+        out.header.frame_id = "odom";
+        out.parent = "base";
+        out.x = x;
+        out.y = y;
+        out.z = 0;
+        out.roll = 0;
+	out.pitch = 0;
+	out.yaw = yaw;
+      transform_publisher_->unlockAndPublish() ;
+      }
   }
 
    if(0)
@@ -708,8 +726,15 @@
     delete publisher_ ;
   publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
 
-  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,1000);
 
+  if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete transform_publisher_ ;
+  transform_publisher_ = new misc_utils::RealtimePublisher <rosTF::TransformArray> ("TransformArray", 1) ;
+
+  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
+
+  transform_publisher_->msg_.set_quaternions_size(1);
+
   if(odom_publish_rate_ > 1e-5)
      odom_publish_delta_t_ = 1.0/odom_publish_rate_;
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5223
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5223&view=rev
Author:   sachinchitta
Date:     2008-10-10 06:16:12 +0000 (Fri, 10 Oct 2008)

Log Message:
-----------
Modified base controller now publishes odom to base transform as TransformArrayBase. This is different from directly publishing libTF transforms (which cannot be done from realtime). 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-10 06:00:12 UTC (rev 5222)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-10 06:16:12 UTC (rev 5223)
@@ -122,24 +122,23 @@
 
 
     /*!
-     * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+     * \brief Specify the commanded velocity for the controller using a libTF::Vector data structure
      *
-     * \param double pos Position command to issue
+     * \param libTF::Vector cmd_vel with cmd_vel.x and cmd_vel.y specifying the forward and sideways speeds respectively while cmd_vel.z specifies the rotational speed.
      */
     void setCommand(libTF::Vector cmd_vel);
 
 
     /*!
-     * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+     * \brief Get the current command. This is the command that is currently being executed by the controller and may differ from the actual command issued because of the imposition of acceleration constraints.
      */
     libTF::Vector getCommand();
 
 
     /*!
      * \brief (a) Updates commands to caster and wheels.
-     *        (b) Computes odometry
-     *        (c) Publishes odometry
-     *  Should be called at regular intervals
+     *         
+     *  Called every timestep in realtime
      */
     virtual void update();
 
@@ -169,35 +168,80 @@
 
     /*!
      * \brief returns odometry data
+     * \param x position estimate from odometry (origin is at the starting point when controller was started)
+     * \param y position estimate from odometry (origin is at the starting point when controller was started)
+     * \param w position estimate from odometry (origin is at the starting point when controller was started)
+     * \param vx velocity estimate from odometry (in the coordinate frame of the base)
+     * \param vy velocity estimate from odometry (in the coordinate frame of the base)
+     * \param vz velocity estimate from odometry (in the coordinate frame of the base)
      */
     void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
 
     /*!
-     * \brief set odometry message
+     * \brief Fill a structure of the form std_msgs::RobotBase2DOdom with odometry data
      */
     void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
 
-    void computeWheelSpeeds();
+    /*!
+     * \brief compute the desired wheel speeds
+     */
+    void computeDesiredWheelSpeeds();
 
-    void setWheelSpeeds();
+    /*!
+     * \brief set the desired wheel speeds in the joint speed controllers
+     */
+    void setDesiredWheelSpeeds();
 
-    void computeCasterSteer();
+    /*!
+     * \brief compute the desired caster steer
+     */
+    void computeDesiredCasterSteer();
 
-    void setCasterSteer();
+    /*!
+     * \brief set the desired caster steer
+     */
+    void setDesiredCasterSteer();
 
-    void computeCommands();
+    /*!
+     * \brief compute the joint commands 
+     */
+    void computeJointCommands();
 
-    void setCommands();
+    /*!
+     * \brief set the joint commands
+     */
+    void setJointCommands();
 
+    /*!
+     * \brief figure out the command for the next time step by interpolating from start to end while staying within rate limits
+     * specified by max_rate
+     * \param start - libTF::Vector specifying the current command
+     * \param end - libTF::Vector specifying the desired command
+     * \param max_rate - libTF::Vector specifying maximum rate of change allowed
+     * \param dT - timestep
+     */
     libTF::Vector interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT);
 
-   NEWMAT::Matrix iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter);
+    /*!
+     * \brief iterative least squares implementation to compute odometry. This implementation computes and returns x where A*x = b
+     * \param A - newmat matrix of size m x n
+     * \param b - newmat matrix of size m x 1
+     * \param weight_type - type of weighting used in the iterations, possible choices include BubeLagan, L1norm, fair, Cauchy, Gaussian
+     * \param max_iter - maximum number of iterations
+     * \return x - the n x 1 result as a NEWMAT matrix
+     */
+    NEWMAT::Matrix iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter);
 
+    /*!
+     * \brief returns the weight matrix given the weight type and residual
+     * \param residual (a m x 1 NEWMAT matrix)
+     * \param weight_type - std::string specification of weight type
+     */
     NEWMAT::Matrix findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type);
 
-    int ils_max_iterations_;
+    int ils_max_iterations_; /** maximum number of iterations for Iterative Least Squares */
 
-    std::string ils_weight_type_;
+    std::string ils_weight_type_; /** std::string specification of weight type used for IRLS*/
 
     /*!
      * \brief Robot representation
@@ -206,7 +250,7 @@
 
     private:
 
-    bool new_cmd_available_;
+    bool new_cmd_available_; /** true when new command received by node */
 
     /*!
      * \brief number of wheels
@@ -239,16 +283,20 @@
 
 
     /*!
-     * \brief speed command vector used internally
+     * \brief speed command vector used internally to represent the current commanded speed
      */
     libTF::Vector cmd_vel_;
 
 
     /*!
-     * \brief Input speed command vector.
+     * \brief Input speed command vector represents the desired speed requested by the node. Note that this may differ from the 
+     * current commanded speed due to acceleration limits imposed by the controller. This 
      */
     libTF::Vector cmd_vel_t_;
 
+    /*!
+     * \brief Input speed command vector represents the desired speed requested by the node. 
+     */
     libTF::Vector desired_vel_;
 
     /*!
@@ -263,32 +311,17 @@
     libTF::Vector base_odom_velocity_;
 
 
-    /*!
-     * \brief Computed the desired steer angle for the caster.
-     */
-    void computeAndSetCasterSteer();
-
-
-    /*!
-     * \brief Computed the desired wheel speeds.
-     */
-    void computeAndSetWheelSpeeds();
-
-
     double wheel_radius_; /** radius of the wheel (filled in from urdf robot model) */
 
-
     std::vector<double> steer_velocity_desired_; /** vector of desired caster steer speeds */
 
     std::vector<double> wheel_speed_cmd_; /** vector of desired wheel speeds */
 
-
     /*!
      * \brief compute the speed of the base for odometry calculations
      */
     void computeBaseVelocity();
 
-
     /*!
      * \brief compute the odometry
      */
@@ -315,6 +348,10 @@
      */
     void getJointValues();
 
+    /*!
+     * \brief function to add parameter to map so it can be initialized easily from the xml file
+     */
+    void addParamToMap(std::string key, double *value); 
 
     std::vector<double> steer_angle_actual_; /** vector of actual caster steer angles */
 
@@ -322,25 +359,20 @@
 
     double last_time_; /** time corresponding to when update was last called */
 
+    double caster_steer_vel_gain_; /** gain specifying the amount of help given by the wheels to the caster steer degree of freedom */
 
-    int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odometry message will be published on ROS */
+    double cmd_received_timestamp_; /** timestamp corresponding to when the command received by the node */
 
-    double caster_steer_vel_gain_;
+    double timeout_; /** timeout specifying time that the controller waits before setting the current velocity command to zero */
 
-    double cmd_received_timestamp_;
+    libTF::Vector max_vel_; /** velocity limits specified externally */
 
-    double timeout_;
+    libTF::Vector max_accel_; /** acceleration limits specified externally */
 
-    libTF::Vector max_vel_; 
+    double MAX_DT_; /** maximum dT used in computation of interpolated velocity command */
 
-    libTF::Vector max_accel_;
-
-    double MAX_DT_;
-
     std::map<std::string, double*> param_map_; /*< map from pointers to the params to string names */
 
-    void addParamToMap(std::string key, double *value);
-
   };
 
   class BaseControllerNode : public Controller
@@ -357,9 +389,25 @@
      */
     ~BaseControllerNode();
 
+    /*!
+     * realtime safe update call
+     */
     void update();
 
+    /*!
+     * \brief initialize the node from a xml config specification
+     */
     bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * \brief get back the odometry values from the controller itself
+     * \param x position computed by odometry 
+     * \param y position computed by odometry
+     * \param theta (yaw) computed by odometry
+     * \param x velocity computed by odometry (in local frame)
+     * \param y velocity computed by odometry (in local frame)
+     * \param theta velocity computed by odometry (in local frame)
+     */
     void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
 
     // Services
@@ -369,6 +417,9 @@
     bool getCommand(pr2_mechanism_controllers::GetBaseCommand::request &req,
                     pr2_mechanism_controllers::GetBaseCommand::response &resp);
 
+    /*
+     * \brief callback function for setting the desired velocity using a topic 
+     */
     void setCommand(double vx, double vy, double vw);
 
     BaseController *c_;
@@ -387,37 +438,20 @@
     ros::thread::mutex ros_lock_;
 
     /*!
-     * \brief frame transform server for publishing base odometry frame
-     */
-    private: rosTFServer *tfs;
-
-    /*!
-     * \brief Set the publish count (number of update ticks between odometry message publishing).
-     */
-    void setPublishCount(int publish_count); //FIXME: use time rather than count
-
-    /*!
      * \brief std_msgs representation of an odometry message
      */
     std_msgs::RobotBase2DOdom odom_msg_;
 
-    /*!
-     * \brief number of update ticks to wait before publishing ROS odom message
-     * defaults to 10.
-     */
-    int odom_publish_count_; //FIXME: use time rather than count
+    double last_time_message_sent_ ;/** last time odometry message was published */
 
-    double last_time_message_sent_ ;
+    double odom_publish_delta_t_; /** time after which odometry message will be published */
 
-    double odom_publish_delta_t_;
-
-    double odom_publish_rate_;
+    double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
            
-    misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
+    misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the odometry msg from the update() realtime loop
 
-    misc_utils::RealtimePublisher <rosTF::TransformArray>* transform_publisher_ ;  //!< Publishes the m_scanner_signal msg from the update() realtime loop
+    misc_utils::RealtimePublisher <rosTF::TransformArray>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
-    int odom_publish_counter_; /** counter - when this exceeds odom_publish_count_, the odomeetry message will be published on ROS */ //FIXME: use time rather than count
   };
 
     /** \brief A namespace ostream overload for displaying parameters */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-10 06:00:12 UTC (rev 5222)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-10 06:16:12 UTC (rev 5223)
@@ -91,26 +91,24 @@
 {
 }
 
-// Set the joint position command
+// Set the base velocity command
 void BaseController::setCommand(libTF::Vector cmd_vel)
 {
 
-   pthread_mutex_lock(&base_controller_lock_);
-   cmd_vel_t_.x = clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
-   cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
-   cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
-   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
-         std::cout << "BaseController:: command received: " << cmd_vel_t_;
-      std::cout << "BaseController:: command current: " << cmd_vel_;
-
-      for(int i=0; i < (int) num_wheels_; i++)
-    {
-     std:: cout << "wheel speed cmd:: " << i << "  " << (base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]) << endl;
-      }
-   cout << "Base Odometry: Velocity " << base_odom_velocity_;
-//   cout << "Base Odometry: Position " << base_odom_position_;
-   new_cmd_available_ = true;
-   pthread_mutex_unlock(&base_controller_lock_);
+  pthread_mutex_lock(&base_controller_lock_);
+  cmd_vel_t_.x = clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+  cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+  cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
+  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+  std::cout << "BaseController:: command received: " << cmd_vel_t_;
+  std::cout << "BaseController:: command current: " << cmd_vel_;
+  std::cout << "Base Odometry: Velocity " << base_odom_velocity_;
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+    std:: cout << "wheel speed cmd:: " << i << "  " << (base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]) << endl;
+  }
+  new_cmd_available_ = true;
+  pthread_mutex_unlock(&base_controller_lock_);
 }
 
 libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
@@ -119,15 +117,6 @@
    result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
    result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
    result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
-
-   //   cout << "Interpolate command start: " << start << endl << endl;
-
-   //   cout << "Interpolate command end: " << end << endl << endl;
-
-   //   cout << "Interpolate command result: " << result << endl << endl;
-
-   //   cout << "dT: " << dT << endl; 
-
    return result;
 }
 
@@ -178,18 +167,18 @@
 
       if(joint_name.find("caster") != string::npos)
       {
-         std::cout << " assigning casters " << joint_name << std::endl;
+//         std::cout << " assigning casters " << joint_name << std::endl;
          base_object.local_id_ = num_casters_;
          base_casters_.push_back(base_object);
          steer_angle_actual_.push_back(0);
          steer_velocity_desired_.push_back(0);
          num_casters_++;
-         cout << "base_casters" << "::  " << base_object;
+//         cout << "base_casters" << "::  " << base_object;
       }
 
       if(joint_name.find("wheel") != string::npos)
       {
-         std::cout << " assigning wheels " << joint_name << std::endl;
+//         std::cout << " assigning wheels " << joint_name << std::endl;
          base_object.local_id_ = num_wheels_;
          if(joint_name.find("_r_") != string::npos)
             base_object.direction_multiplier_ = 1;
@@ -243,14 +232,10 @@
 
    std::cout << " base controller name: " << config->Attribute("name") << std::endl;
    TiXmlElement *elt = config->FirstChildElement("controller");
-//  std::cout << " child " << std::endl << *elt << std::endl;
    std::vector<JointControlParam> jcp_vec;
    JointControlParam jcp;
    while (elt){
       TiXmlElement *jnt = elt->FirstChildElement("joint");
-      //std::cout << "joint snippit" << std::endl << *jnt << std::endl;
-
-      // TODO: error check if xml attributes/elements are missing
       jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
       jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
       jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
@@ -261,7 +246,7 @@
 
       std::cout << "name:" << jcp.joint_name << std::endl;
       std::cout << "controller type:" << jcp.control_type << std::endl;
-      std::cout << " sub controller : " << elt->Attribute("name") << std::endl;
+      std::cout << "sub controller: " << elt->Attribute("name") << std::endl;
 
       elt = elt->NextSiblingElement("controller");
    }
@@ -290,7 +275,6 @@
          }
       }
       std::cout << "*************************************" << std::endl;
-//    std::cout << " sub map : " << elt->Attribute("name") << std::endl;
       elt = config->NextSiblingElement("map");
    }
 
@@ -304,16 +288,6 @@
    cout << "max_y_accel  " << (max_accel_.y) << endl;
    cout << "max_yaw_accel  " << (max_accel_.z) << endl;
 
-   cout << "kp_speed  " << kp_speed_ << endl;
-   cout << "kp_caster_steer  " << caster_steer_vel_gain_ << endl;
-   cout << "timeout  " << timeout_ << endl;
-   cout << "max_x_dot  " << (max_vel_.x) << endl;
-   cout << "max_y_dot  " << (max_vel_.y) << endl;
-   cout << "max_yaw_dot  " << (max_vel_.z) << endl;
-   cout << "max_x_accel  " << (max_accel_.x) << endl;
-   cout << "max_y_accel  " << (max_accel_.y) << endl;
-   cout << "max_yaw_accel  " << (max_accel_.z) << endl;
-
    init(jcp_vec,robot_state);
    return true;
 }
@@ -330,11 +304,6 @@
 
    for(int i=0; i < num_wheels_; i++)
       wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
-
-//   for(int i=0; i < num_casters_; i++)
-//     std::cout << " caster angles " << i << " : " << steer_angle_actual_[i] << std::endl;
-//   for(int i=0; i < num_wheels_; i++)
-//     std::cout << " wheel rates " << i << " : " << wheel_speed_actual_[i] << std::endl;
 }
 
 void BaseController::computeWheelPositions()
@@ -344,16 +313,10 @@
    for(int i=0; i < num_wheels_; i++)
    {
       steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-//    res1 = rotate2D(base_wheels_[i].pos_,steer_angle);
-//    cout << "init position" << base_wheels_[i].pos_.x << " " << base_wheels_[i].pos_.y << " " << base_wheels_[i].pos_.z << endl;
       res1 = base_wheels_[i].pos_.rot2D(steer_angle);
-//    cout << "rotated position" << res1;
       res1 += base_wheels_[i].parent_->pos_;
-//    cout << "added position" << res1;
       base_wheels_position_[i] = res1;
-      //cout << "base_wheels_position_(" << i << ")" << base_wheels_position_[i];
    }
-//  exit(-1);
 }
 
 void BaseController::update()
@@ -371,7 +334,6 @@
          new_cmd_available_ = false;
          pthread_mutex_unlock(&base_controller_lock_);
       }
-   //  std::cout << "BaseController::update current command: " << cmd_vel_ << std::endl;
    }
 
    if((current_time - cmd_received_timestamp_) > timeout_)
@@ -386,16 +348,10 @@
 
    getJointValues();
 
-   computeCommands();
+   computeJointCommands();
 
-   setCommands();
+   setJointCommands();
 
-/*  computeWheelPositions();
-
-computeAndSetCasterSteer();
-
-computeAndSetWheelSpeeds();
-*/
    computeOdometry(current_time);
 
    updateJointControllers();
@@ -404,23 +360,23 @@
 }
 
 
-void BaseController::computeCommands()
+void BaseController::computeJointCommands()
 {
    computeWheelPositions();
 
-   computeCasterSteer();
+   computeDesiredCasterSteer();
 
-   computeWheelSpeeds();  
+   computeDesiredWheelSpeeds();  
 }
 
-void BaseController::setCommands()
+void BaseController::setJointCommands()
 {
-   setCasterSteer();
+   setDesiredCasterSteer();
 
-   setWheelSpeeds();
+   setDesiredWheelSpeeds();
 }
 
-void BaseController::computeCasterSteer()
+void BaseController::computeDesiredCasterSteer()
 {
    libTF::Vector result;
 
@@ -433,7 +389,6 @@
 
       steer_angle_desired = atan2(result.y,result.x);
       steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
-//    printf("Steering cmd choices: %d, %f, %f\n",i,steer_angle_desired,steer_angle_desired_m_pi);
 
       error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
       error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
@@ -444,18 +399,16 @@
          steer_angle_desired = steer_angle_desired_m_pi;
       }
       steer_velocity_desired_[i] =  -kp_speed_*error_steer;
-
-      //  printf("Steering cmd: %d, %f, %f, %f\n",i,steer_angle_desired,steer_angle_actual_[i],error_steer);
    }
 }
 
-void BaseController::setCasterSteer()
+void BaseController::setDesiredCasterSteer()
 {
    for(int i=0; i < num_casters_; i++)
       base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
 }
 
-void BaseController::computeWheelSpeeds()
+void BaseController::computeDesiredWheelSpeeds()
 {
    libTF::Vector wheel_point_velocity;
    libTF::Vector wheel_point_velocity_projected;
@@ -472,19 +425,14 @@
       caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
       steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
       wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
-//    cout << "gain " << caster_steer_vel_gain_ << endl;
-//    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
 
       wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-//    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
       wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
       wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-
-//    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd_[i] << ", actual wheel speed" << wheel_speed_actual_[i] << ", caster: " << wheel_caster_steer_component.x/wheel_radius_ << ", caster_steer " << caster_2d_velocity.z << std::endl;
    }
 }
 
-void BaseController::setWheelSpeeds()
+void BaseController::setDesiredWheelSpeeds()
 {
    for(int i=0; i < (int) num_wheels_; i++)
    {
@@ -492,54 +440,7 @@
    }
 }
 
-void BaseController::computeAndSetCasterSteer()
-{
-   libTF::Vector result;
-   double steer_angle_desired;
-   double error_steer;
-   for(int i=0; i < num_casters_; i++)
-   {
-      result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
-      steer_angle_desired = atan2(result.y,result.x);
-      //     steer_angle_desired =  modNPiBy2(steer_angle_desired);//Clean steer Angle
 
-      error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
-      steer_velocity_desired_[i] = -kp_speed_*error_steer;
-      //    std::cout << "setting steering velocity " << i << " : " << steer_velocity_desired_[i] << " kp: " << kp_speed_ << "error_steer" << error_steer << std::endl;
-      base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
-   }
-}
-
-void BaseController::computeAndSetWheelSpeeds()
-{
-   libTF::Vector wheel_point_velocity;
-   libTF::Vector wheel_point_velocity_projected;
-   libTF::Vector wheel_caster_steer_component;
-   libTF::Vector caster_2d_velocity;
-
-   caster_2d_velocity.x = 0;
-   caster_2d_velocity.y = 0;
-   caster_2d_velocity.z = 0;
-
-   double wheel_speed_cmd = 0;
-   double steer_angle_actual = 0;
-   for(int i=0; i < (int) num_wheels_; i++)
-   {
-
-      caster_2d_velocity.z = steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
-      steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
-      wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
-      //    cout << "wheel_point_velocity" << wheel_point_velocity << ",pos::" << base_wheels_position_[i] << ",cmd::" << cmd_vel_ << endl;
-      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-      //    wheel_point_velocity_projected = rotate2D(wheel_point_velocity,-steer_angle_actual);
-      wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-      wheel_speed_cmd = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-      //    std::cout << "setting wheel speed " << i << " : " << wheel_speed_cmd << " r:" << wheel_radius_ << std::endl;
-      base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd);
-   }
-
-}
-
 void BaseController::updateJointControllers()
 {
    for(int i=0; i < num_wheels_; i++)
@@ -561,198 +462,9 @@
    odom_msg_.vel.x  = base_odom_velocity_.x;
    odom_msg_.vel.y  = base_odom_velocity_.y;
    odom_msg_.vel.th = base_odom_velocity_.z;
-
-//   cout << "Base Odometry: Velocity " << base_odom_velocity_;
-//   cout << "Base Odometry: Position " << base_odom_position_;
 }
 
-ROS_REGISTER_CONTROLLER(BaseControllerNode)
-   BaseControllerNode::BaseControllerNode() : odom_publish_count_(10), odom_publish_counter_(0)
-{
-   c_ = new BaseController();
-   node = ros::node::instance();
-   last_time_message_sent_ = 0.0;
-   odom_publish_rate_ = 100.0;
-   odom_publish_delta_t_ = 1.0/odom_publish_rate_;
-   publisher_ = NULL;
-   transform_publisher_ = NULL;
-}
 
-BaseControllerNode::~BaseControllerNode()
-{
-   node->unadvertise_service(service_prefix + "/set_command");
-   node->unadvertise_service(service_prefix + "/get_actual");
-//   node->unadvertise("odom");
-   node->unsubscribe("cmd_vel");
-
-   publisher_->stop();
-   delete publisher_;
-   delete transform_publisher_;
-   delete c_;
-}
-
-void BaseControllerNode::setPublishCount(int publish_count)
-{
-   odom_publish_count_ = publish_count;
-};
-
-void BaseControllerNode::update()
-{
-   double time = c_->robot_state_->hw_->current_time_;
-
-   c_->update();
-   c_->setOdomMessage(odom_msg_);
-
-   //if(0)
-	  if (time-last_time_message_sent_ >= odom_publish_delta_t_) // send odom message
-  {
-    if (publisher_->trylock())
-    {
-      c_->setOdomMessage(publisher_->msg_);
-      publisher_->unlockAndPublish() ;
-      last_time_message_sent_ = time;
-    }
-
-    if (transform_publisher_->trylock())
-      {
-      double x=0,y=0,yaw=0,vx,vy,vyaw;
-      this->getOdometry(x,y,yaw,vx,vy,vyaw);
-      rosTF::TransformEuler &out = transform_publisher_->msg_.eulers[0];
-        out.header.stamp.from_double(time);
-        out.header.frame_id = "odom";
-        out.parent = "base";
-        out.x = x;
-        out.y = y;
-        out.z = 0;
-        out.roll = 0;
-	out.pitch = 0;
-	out.yaw = yaw;
-      transform_publisher_->unlockAndPublish() ;
-      }
-  }
-
-   if(0)
-//  if(odom_publish_counter_ > odom_publish_count_) // FIXME: switch to time based rate limiting
-   {
-      (ros::g_node)->publish("odom", odom_msg_);
-      odom_publish_counter_ = 0;
-
-      // FIXME: should this be in here?
-      /***************************************************************/
-      /*                                                             */
-      /*  frame transforms                                           */
-      /*                                                             */
-      /*  TODO: should we send z, roll, pitch, yaw? seems to confuse */
-      /*        localization                                         */
-      /*                                                             */
-      /***************************************************************/
-      double x=0,y=0,yaw=0,vx,vy,vyaw;
-      this->getOdometry(x,y,yaw,vx,vy,vyaw);
-      this->tfs->sendInverseEuler("odom",
-                                  "base",
-                                  x, y, 0,
-                                  yaw, 0, 0,
-                                  odom_msg_.header.stamp);
-
-   }
-
-   odom_publish_counter_++;
-
-}
-
-bool BaseControllerNode::setCommand(
-   pr2_mechanism_controllers::SetBaseCommand::request &req,
-   pr2_mechanism_controllers::SetBaseCommand::response &resp)
-{
-   libTF::Vector command;
-   command.x = req.x_vel;
-   command.y = req.y_vel;
-   command.z = req.theta_vel;
-   c_->setCommand(command);
-   command = c_->getCommand();
-   resp.x_vel = command.x;
-   resp.y_vel = command.y;
-   resp.theta_vel = command.z;
-      std::cout << "BaseController:: odom_publish_rate: " << odom_publish_rate_ << endl;
-   return true;
-}
-
-void BaseControllerNode::setCommand(double vx, double vy, double vw)
-{
-   libTF::Vector command;
-   command.x = vx;
-   command.y = vy;
-   command.z = vw;
-   c_->setCommand(command);
-      std::cout << "BaseController:: odom_publish_rate: " << odom_publish_rate_ << endl;
-   //std::cout << "command received : " << vx << "," << vy << "," << vw << std::endl;
-}
-
-
-bool BaseControllerNode::getCommand(
-   pr2_mechanism_controllers::GetBaseCommand::request &req,
-   pr2_mechanism_controllers::GetBaseCommand::response &resp)
-{
-   libTF::Vector command;
-   command = c_->getCommand();
-   resp.x_vel = command.x;
-   resp.y_vel = command.y;
-   resp.theta_vel = command.z;
-
-   return true;
-}
-
-bool BaseControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
-{
-   service_prefix = config->Attribute("name");
-
-   assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
-
-   if(!c_->initXml(robot_state, config))
-      return false;
-
-   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
-   node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
-
-//   node->advertise<std_msgs::RobotBase2DOdom>("odom",10);
-
-  // receive messages from 2dnav stack
-  node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
-
-   // for publishing odometry frame transforms odom
-//   this->tfs = new rosTFServer(*node); //, true, 1 * 1000000000ULL, 0ULL);
-
-  if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
-    delete publisher_ ;
-  publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
-
-
-  if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
-    delete transform_publisher_ ;
-  transform_publisher_ = new misc_utils::RealtimePublisher <rosTF::TransformArray> ("TransformArray", 1) ;
-
-  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
-
-  transform_publisher_->msg_.set_quaternions_size(1);
-
-  if(odom_publish_rate_ > 1e-5)
-     odom_publish_delta_t_ = 1.0/odom_publish_rate_;
-
-   return true;
-}
-
-void BaseControllerNode::CmdBaseVelReceived()
-{
-   this->ros_lock_.lock();
-   this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
-   this->ros_lock_.unlock();
-}
-
-void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
-{
-   c_->getOdometry(x,y,w,vx,vy,vw);
-}
-
 void BaseController::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
 {
    x = base_odom_position_.x;
@@ -779,7 +491,6 @@
 void BaseController::computeOdometry(double time)
 {
    double dt = time-last_time_;
-//   libTF::Vector base_odom_delta = rotate2D(base_odom_velocity_*dt,base_odom_position_.z);
 
    computeBaseVelocity();
 
@@ -825,8 +536,6 @@
    base_odom_velocity_.x = (double)D.element(0,0);
    base_odom_velocity_.y = (double)D.element(1,0);
    base_odom_velocity_.z = (double)D.element(2,0);
-
-//  cout << "Base odom velocity " << base_odom_velocity_ << endl;
 }
 
 Matrix BaseController::pseudoInverse(const Matrix M)
@@ -860,7 +569,6 @@
 
    weight_matrix = ident_matrix;
 
-//  cout << "num_wheels: " << num_wheels_ << endl;
    for(int i=0; i < max_iter; i++)
    {
       a_fit = weight_matrix * A;
@@ -868,9 +576,6 @@
       x_fit = pseudoInverse(a_fit)*b_fit;
       residual = b - A * x_fit;
 
-//    cout << "Residual::" << residual << endl;
-
-
       for(int j=1; j <= num_wheels_; j++)
       {
          int fw = 2*j-1;
@@ -886,15 +591,7 @@
             residual(sw,1) = residual(fw,1);
          }
       }
-
-//    cout << "afit::" << a_fit << endl << endl;
-//    cout << "bfit::" << b_fit << endl << endl;
-//    cout << "xfit::" << endl << x_fit << endl;
-//    cout << "Residual::" << residual << endl;
-
       weight_matrix = findWeightMatrix(residual,weight_type);
-
-//    cout << "weight_matrix" << endl << weight_matrix  << endl << endl;
    }
    return x_fit;
 };
@@ -951,3 +648,151 @@
 };
 
 
+
+
+
+ROS_REGISTER_CONTROLLER(BaseControllerNode)
+
+BaseControllerNode::BaseControllerNode()
+{
+   c_ = new BaseController();
+   node = ros::node::instance();
+   last_time_message_sent_ = 0.0;
+   odom_publish_rate_ = 100.0;
+   odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+   publisher_ = NULL;
+   transform_publisher_ = NULL;
+}
+
+BaseControllerNode::~BaseControllerNode()
+{
+   node->unadvertise_service(service_prefix + "/set_command");
+   node->unadvertise_service(service_prefix + "/get_actual");
+   node->unsubscribe("cmd_vel");
+
+   publisher_->stop();
+   transform_publisher_->stop();
+   delete publisher_;
+   delete transform_publisher_;
+   delete c_;
+}
+
+void BaseControllerNode::update()
+{
+  double time = c_->robot_state_->hw_->current_time_;
+
+  c_->update();
+  c_->setOdomMessage(odom_msg_);
+
+  if (time-last_time_message_sent_ >= odom_publish_delta_t_) // send odom message
+  {
+    if (publisher_->trylock())
+    {
+      c_->setOdomMessage(publisher_->msg_);
+      publisher_->unlockAndPublish() ;
+      last_time_message_sent_ = time;
+    }
+
+    if (transform_publisher_->trylock())
+    {
+      double x=0,y=0,yaw=0,vx,vy,vyaw;
+      this->getOdometry(x,y,yaw,vx,vy,vyaw);
+      rosTF::TransformEuler &out = transform_publisher_->msg_.eulers[0];
+      out.header.stamp.from_double(time);
+      out.header.frame_id = "odom";
+      out.parent = "base";
+      out.x = x;
+      out.y = y;
+      out.z = 0;
+      out.roll = 0;
+      out.pitch = 0;
+      out.yaw = yaw;
+      transform_publisher_->unlockAndPublish() ;
+    }
+  }
+}
+
+bool BaseControllerNode::setCommand(
+   pr2_mechanism_controllers::SetBaseCommand::request &req,
+   pr2_mechanism_controllers::SetBaseCommand::response &resp)
+{
+   libTF::Vector command;
+   command.x = req.x_vel;
+   command.y = req.y_vel;
+   command.z = req.theta_vel;
+   c_->setCommand(command);
+   command = c_->getCommand();
+   resp.x_vel = command.x;
+   resp.y_vel = command.y;
+   resp.theta_vel = command.z;
+   return true;
+}
+
+void BaseControllerNode::setCommand(double vx, double vy, double vw)
+{
+   libTF::Vector command;
+   command.x = vx;
+   command.y = vy;
+   command.z = vw;
+   c_->setCommand(command);
+   std::cout << "BaseController:: odom_publish_rate: " << odom_publish_rate_ << endl;
+}
+
+
+bool BaseControllerNode::getCommand(
+   pr2_mechanism_controllers::GetBaseCommand::request &req,
+   pr2_mechanism_controllers::GetBaseCommand::response &resp)
+{
+   libTF::Vector command;
+   command = c_->getCommand();
+   resp.x_vel = command.x;
+   resp.y_vel = command.y;
+   resp.theta_vel = command.z;
+
+   return true;
+}
+
+bool BaseControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
+{
+   service_prefix = config->Attribute("name");
+
+   assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
+
+   if(!c_->initXml(robot_state, config))
+      return false;
+
+   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
+   node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
+
+ 
+  if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete publisher_ ;
+  publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+
+
+  if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete transform_publisher_ ;
+  transform_publisher_ = new misc_utils::RealtimePublisher <rosTF::TransformArray> ("TransformArrayBase", 1) ;
+
+  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
+
+  transform_publisher_->msg_.set_eulers_size(1);
+
+  if(odom_publish_rate_ > 1e-5)
+     odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+
+   return true;
+}
+
+void BaseControllerNode::CmdBaseVelReceived()
+{
+   this->ros_lock_.lock();
+   this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+   this->ros_lock_.unlock();
+}
+
+void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
+{
+   c_->getOdometry(x,y,w,vx,vy,vw);
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5250
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5250&view=rev
Author:   jleibs
Date:     2008-10-10 21:12:34 +0000 (Fri, 10 Oct 2008)

Log Message:
-----------
final head pan tilt controller

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox	2008-10-10 21:07:53 UTC (rev 5249)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox	2008-10-10 21:12:34 UTC (rev 5250)
@@ -5,6 +5,15 @@
 
 PR2 Specific Controllers
 
+Here are the controllers which are currently implemented:
+ \li \ref controller::ArmPositionController "Arm Position Controller"
+ \li \ref controller::ArmVelocityController "Arm Velocity Controller"
+ \li \ref controller::ArmDynamicsController "Arm Dynamics Controller"
+ \li \ref controller::HeadPanTiltController "Head Pan Tilt Controller"
+ \li \ref controller::LaserScannerController "Laser Scanner Controller"
+ \li \ref controller::BaseController "Base Controller"
+
+
 @section topic PR2 Controllers ROS Topics
  - ROS topics published by controllers
      <table border="1">

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-10 21:07:53 UTC (rev 5249)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-10 21:12:34 UTC (rev 5250)
@@ -32,11 +32,15 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+// Original version: Melonee Wise <mwise@...>
+
 #pragma once
 
+// ROS stuff
 #include <ros/node.h>
 #include <rosthread/mutex.h>
 
+// Controllers
 #include <mechanism_model/controller.h>
 #include <mechanism_model/joint.h>
 #include <robot_mechanism_controllers/joint_position_controller.h>
@@ -46,160 +50,168 @@
 #include <pr2_mechanism_controllers/GetJointCmd.h>
 #include <pr2_mechanism_controllers/TrackPoint.h>
 
-#include <pr2_mechanism_controllers/SetJointGains.h>
-#include <pr2_mechanism_controllers/GetJointGains.h>
-
-
 // Math utils
 #include <math_utils/angles.h>
-
 #include <rosTF/rosTF.h>
-// #include <urdf/URDF.h>
 
 namespace controller
 {
-
-// The maximum number of joints expected in an arm.
+// The maximum number of joints expected in a head.
 static const int MAX_HEAD_JOINTS = 2;
 
+/***************************************************/
+/*! \class controller::HeadPanTiltController
+    \brief Head Pan Tilt Controller
+
+    This class closes the loop around the joint angles.
+
+    Example config:
+
+    <controller name="head_controller" type="HeadPanTiltControllerNode">
+      <listen_topic name="head_commands" />
+    
+      <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionController">
+        <joint name="head_pan_joint" >
+          <pid p="1.5" d="0.1" i="0.3" iClamp="0.2" />
+        </joint>
+      </controller>
+      
+      <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionController">
+        <joint name="head_tilt_joint" >
+          <pid p="0.8" d="0.05" i="0.1" iClamp="0.1" />
+        </joint>
+      </controller>
+    </controller>  
+*/
+/***************************************************/
+
 class HeadPanTiltController : public Controller
 {
 public:
 
   /*!
-   * \brief Default Constructor of the JointController class.
+   * \brief Default Constructor of the HeadPanTiltController class.
    *
    */
   HeadPanTiltController();
 
   /*!
-   * \brief Destructor of the JointController class.
+   * \brief Destructor of the HeadPanTiltController class.
    */
   virtual ~HeadPanTiltController();
 
   /*!
    * \brief Functional way to initialize limits and gains.
    *
-  */
+   */
   bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
 
-
   /*!
-   * \brief Sets a goal for some joint, specified as a pair of (joint values, joint names)
+   * \brief Sets goals for some joint, specified as a pair of (joint values, joint names).
    *
-   * \param j_values the positions of the joints
-   * \param j_names the names of the joints
-  */
+   * \param j_values the positions of the joints.
+   * \param j_names the names of the joints.
+   */
   void setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names);
 
-//  /**
-//   * @brief Overloaded method for convenience
-//   * @param req
-//   */
-//  void setJointCmd(pr2_mechanism_controllers::SetJointPosCmd::request &req);
-
+  /*!
+   * \brief Returns the commanded joint poisitions, specified as a pair of (joint values, joint names).
+   *
+   * \param cmd names and positions.
+   */
   void getJointCmd(pr2_mechanism_controllers::JointCmd & cmd) const;
 
   /*!
-   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
-  */
-//  void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+   * \brief Returns the position of the controller in the vector.
+   *
+   * \param name the name of the joint being controlled by the controller (i.e. head_pan_joint).
+   */
+  int getJointControllerByName(std::string name);
 
-//  void getCurrentConfiguration(std::vector<double> &);
-
   /*!
-     * \brief Issues commands to the joint. Should be called at regular intervals
-  */
+   * \brief Issues commands to the joint.
+   */
   virtual void update(void); // Real time safe.
+  
+  unsigned int num_joints_; /**< Number of joints. */
+  controller::JointPositionController* getJointPositionControllerByName(std::string name); /**< Joints we're controlling. */
+  std::vector<JointPositionController *> joint_position_controllers_; /**< Vector of the joint controllers. */ 
 
-  controller::JointPositionController* getJointPositionControllerByName(std::string name);
 
-  unsigned int num_joints_;
-  std::vector<JointPositionController *> joint_position_controllers_;  
-  int getJointControllerByName(std::string name);
-
 private:
 
-  double last_time_;
+  double last_time_;            /**< The last time. */
+  std::vector<double> set_pts_; /**< The vector of joint set_pts. */
+  mechanism::Robot* robot_;     /**< The robot we're controlling. */
 
-  
+  /*!
+   * \brief Issues commands to the joint.
+   */
+  void updateJointControllers(void); 
 
-  // Goal of the joints
-  std::vector<double> set_pts_;
+};
 
-  mechanism::Robot* robot_;
+/***************************************************/
+/*! \class controller::HeadPanTiltControllerNode
+    \brief Head Pan Tilt Controller Node
 
-  void updateJointControllers(void);
+    This class sets the joint angles given a point or
+    joint angles.
 
-  
-  
-  
+*/
+/***************************************************/
 
-};
-
-/** @class HeadPanTiltControllerNode
- *  @brief ROS interface for the head controller.
- *
- *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the head:
- *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
- *      <listen_topic name="the name of my message" />
- *      (only one topic can be specified)
- *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
- *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
- *
- */
 class HeadPanTiltControllerNode : public Controller
 {
   public:
+    
     /*!
-    * \brief Default Constructor
-    *
-    */
+     * \brief Default Constructor
+     */
     HeadPanTiltControllerNode();
 
     /*!
-    * \brief Destructor
-    */
+     * \brief Destructor
+     */
     ~HeadPanTiltControllerNode();
 
     void update();
 
     bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
 
-    /** @brief sets a command for all the joints managed by the controller at once
-     * This is a lightweight version of setJointPosHeadless when the caller knows the order of the joints.
-     * @note this service should not be used. use setJointPosHeadless instead
-     * This service returns immediately
-     * @param req an array of position
-     * @param resp The response is empty
-     * @return
+    /*!
+     * \brief Sets a command for all the joints managed by the controller at once.
+     * 
+     * \param req (names, positions)
+     * \param resp (names, positions)
      */
     bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
                     pr2_mechanism_controllers::SetJointCmd::response &resp);
 
-    /** @brief service that returns the goal of the controller
-     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
-     * @param req
-     * @param resp the response, contains a JointPosCmd message with the goal of the controller
-     * @return
+    /*!
+     * \brief Gets the commands for all the joints managed by the controller at once.
+     * 
+     * \param req 
+     * \param resp (positions)
      */
     bool getJointCmd(pr2_mechanism_controllers::GetJointCmd::request &req,
                         pr2_mechanism_controllers::GetJointCmd::response &resp);
-    
+    /*!
+     * \brief Tracks a point in a specified frame.
+     * 
+     * \param req  trackpoint (header, point)
+     * \param resp (pan_angle, tilt_angle)
+     */
     bool trackPoint(pr2_mechanism_controllers::TrackPoint::request &req,                                    
                      pr2_mechanism_controllers::TrackPoint::response &resp);
   private:
-    pr2_mechanism_controllers::JointCmd msg_;   //The message used by the ROS callback
-    HeadPanTiltController *c_;
-    std::string service_prefix;
-    /*
-     * \brief pointer to ros node
-     */
-    ros::node *node;
-    rosTFClient TF;
 
+    HeadPanTiltController *c_;      /**< The controller. */
+    std::string service_prefix;     /**< The service name. */
+    ros::node *node;                /**< The node. */
+    rosTFClient TF;                 /**< The transform for converting from point to head and tilt frames. */
+
 };
-
 }
 
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-10 21:07:53 UTC (rev 5249)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-10 21:12:34 UTC (rev 5250)
@@ -64,6 +64,7 @@
     std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
     assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPositionController"));
     joint_position_controllers_.push_back(jpc);
+
     if(!jpc->initXml(robot, elt))
       return false;
 
@@ -132,7 +133,6 @@
   return -1;
 }
 
-
 void HeadPanTiltController::update(void)
 {
   for(unsigned int i=0; i < num_joints_;++i)
@@ -143,15 +143,12 @@
   updateJointControllers();
 }
 
-
-
 void HeadPanTiltController::updateJointControllers(void)
 {
   for(unsigned int i=0;i<num_joints_;++i)
     joint_position_controllers_[i]->update();
 }
 
-
 //------ Head controller node --------
 
 ROS_REGISTER_CONTROLLER(HeadPanTiltControllerNode)
@@ -254,8 +251,8 @@
   
      
   c_->setJointCmd(pos,names);
-  resp.pan_angle = mes_pan_angle;//head_pan_angle;
-  resp.tilt_angle = mes_tilt_angle;//head_tilt_angle;
+  resp.pan_angle = head_pan_angle;
+  resp.tilt_angle =head_tilt_angle;
   return true;
 }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5285
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5285&view=rev
Author:   mmwise
Date:     2008-10-11 01:29:51 +0000 (Sat, 11 Oct 2008)

Log Message:
-----------
updating documentation

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox	2008-10-11 01:22:52 UTC (rev 5284)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox	2008-10-11 01:29:51 UTC (rev 5285)
@@ -6,50 +6,105 @@
 PR2 Specific Controllers
 
 Here are the controllers which are currently implemented:
+ \li \ref controller::ArmDynamicsController "Arm Dynamics Controller"
  \li \ref controller::ArmPositionController "Arm Position Controller"
  \li \ref controller::ArmVelocityController "Arm Velocity Controller"
- \li \ref controller::ArmDynamicsController "Arm Dynamics Controller"
+ \li \ref controller::BaseController "Base Controller"
  \li \ref controller::HeadPanTiltController "Head Pan Tilt Controller"
  \li \ref controller::LaserScannerController "Laser Scanner Controller"
- \li \ref controller::BaseController "Base Controller"
 
 
+<hr>
+
 @section topic PR2 Controllers ROS Topics
  - ROS topics published by controllers
-     <table border="1">
-     <tr><th>  Topic Name                          </th> <th> Message Type                         </th>  <th> Plugin Name             </th> <th>  Description                                               </th>  </tr>
-     <tr><td> \b odom                              </td> <td>  std_msgs::RobotBase2DOdom           </td>  <td>  base_controller.h      </td> <td>   odometry information from base caster and wheels.        </td>  </tr>
-     </table><br>
+   - Base Controller
+    <table border="1">
+    <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+    <tr><td>\ref controller::BaseController::setOdomMessage "odom"</td>
+        <td><a href="http://pr.willowgarage.com/pr-docs/ros-packages/std_msgs/html/classstd__msgs_1_1RobotBase2DOdom.html">RobotBase2DOdom </a> </td>
+        <td>Odometry information from base caster and wheels. </td></tr>
+    </table><br>
 
  - ROS topics subscribed by controllers
-     <table border="1">
-     <tr><th>  Topic Name                          </th> <th> Message Type                         </th>  <th> Plugin Name             </th> <th>  Description                                               </th>  </tr>
-     <tr><td> \e cmd_vel                           </td> <td>  std_msgs::BaseVel                   </td>  <td>  base_controller.h      </td> <td>   velocity commands for the base (vx,vy,vw).               </td>  </tr>
-     </table><br>
-  - arm position controllers:
-     <table border="1">
-     <tr><th>  Topic Name                          </th> <th> Message Type                         </th>  <th> Plugin Name             </th> <th>  Description                                               </th>  </tr>
-     <tr><td> \e left_arm_commands                 </td> <td> pr2_mechanism_controllers::JointPosCmd </td><td>arm_position_controller.h</td> <td>   Arm configuration command for each joint.                </td>  </tr>
-     <tr><td> \e right_arm_commands                </td> <td> pr2_mechanism_controllers::JointPosCmd </td><td>arm_position_controller.h</td> <td>   Arm configuration command for each joint.                </td>  </tr>
-     </table><br>
+  - Arm Position Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::ArmPositionControllerNode::setJointPosSingleHeadless_cb "(left/right)_arm_commands"</td>
+      <td>\ref pr2_mechanism_controllers::msg::_JointPosCmd::JointPosCmd "JointPosCmd"</td>
+      <td>Arm configuration command for each joint.</td></tr>
+  </table><br>
+  - Base Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::BaseControllerNode::CmdBaseVelReceived "cmd_vel"</td>
+      <td><a href="http://pr.willowgarage.com/pr-docs/ros-packages/std_msgs/html/classstd__msgs_1_1BaseVel.html">BaseVel </a> </td>
+      <td>Velocity commands for the base (v_x,v_y,v_theta).</td></tr>
+  </table><br>
 
+<hr>
 @section services PR2 Controllers ROS Services
- - ROS service published by controllers
-     <table border="1">
-     <tr><th>  Topic Name                                </th> <th> Message Type                              </th>  <th> Plugin Name              </th> <th>  Description                                   </th>  </tr>
-     <tr><td> \b base_controller/set_command             </td> <td>pr2_mechanism_controllers::SetBaseCommand  </td>  <td>  base_controller.h       </td> <td>   set base command.                            </td>  </tr>
-     <tr><td> \b base_controller/get_actual              </td> <td>pr2_mechanism_controllers::GetBaseCommand  </td>  <td>  base_controller.h       </td> <td>   get base command.                            </td>  </tr>
-     <tr><td> \b tilt_laser_controller/set_command       </td> <td>robot_mechanism_controllers::SetCommand    </td>  <td>laser_scanner_controller.h</td> <td>   set controller command.                      </td>  </tr>
-     <tr><td> \b tilt_laser_controller/get_actual        </td> <td>robot_mechanism_controllers::GetCommand    </td>  <td>laser_scanner_controller.h</td> <td>   get controller command.                      </td>  </tr>
-     <tr><td> \b tilt_laser_controller/set_profile       </td> <td>pr2_mechanism_controllers::SetProfile      </td>  <td>laser_scanner_controller.h</td> <td>   set sweep profile.                           </td>  </tr>
-     <tr><td> \b left_arm_controller/set_command         </td> <td>pr2_mechanism_controllers::SetJointTarget  </td>  <td> arm_position_controller.h</td> <td>   set arm joint targets.                       </td>  </tr>
-     <tr><td> \b left_arm_controller/set_command_array   </td> <td>pr2_mechanism_controllers::SetJointPosCmd  </td>  <td> arm_position_controller.h</td> <td>   set arm joint positions.                     </td>  </tr>
-     <tr><td> \b left_arm_controller/get_command         </td> <td>pr2_mechanism_controllers::GetJointPosCmd  </td>  <td> arm_position_controller.h</td> <td>   get arm joint positions.                     </td>  </tr>
-     <tr><td> \b left_arm_controller/set_target          </td> <td>pr2_mechanism_controllers::SetJointTarget  </td>  <td> arm_position_controller.h</td> <td>   set arm joint targets.                       </td>  </tr>
-     </table><br>
-     @todo rename base_controller/get_actual to base_controller/get_command.
-           update documentation for arm position controller services.
 
+  - Arm Position Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::ArmPositionControllerNode::setJointPosHeadless "(left/right)_arm_controller/set_command"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetJointTarget::SetJointTarget "SetJointTarget"</td>
+      <td>Set arm joint targets.</td></tr>
+  <tr><td>\ref controller::ArmPositionControllerNode::setJointPosSrv "(left/right)_arm_controller/set_command_array"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetJointCmd::SetJointCmd "SetJointCmd"</td>
+      <td>Set arm joint positions.</td></tr>
+  <tr><td>\ref controller::ArmPositionControllerNode::getJointPosCmd "(left/right)_arm_controller/get_command"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_GetJointPosCmd::GetJointPosCmd "GetJointPosCmd"</td>
+      <td>Get arm joint positions. </td></tr>
+  <tr><td>\ref controller::ArmPositionControllerNode::setJointPosTarget "(left/right)_arm_controller/set_target"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetJointTarget::SetJointTarget "SetJointTarget"</td>
+      <td>Set arm joint targets. </td></tr>
+  </table><br>
+  
+  - Base Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::BaseControllerNode::setCommand "base_controller/set_command"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetBaseCommand::SetBaseCommand "SetBaseCommand"</td>
+      <td>Sets the base command v_x, v_y, v_theta. </td></tr>
+  <tr><td>\ref controller::BaseControllerNode::getCommand "base_controller/get_command"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_GetBaseCommand::GetBaseCommand "GetBaseCommand"</td>
+      <td>Gets the base command v_x, v_y, v_theta. </td></tr>
+  </table><br>
+  
+  - Head Pan Tilt Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::HeadPanTiltControllerNode::setJointCmd "head_controller/set_command_array"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetJointCmd::SetJointCmd "SetJointCmd"</td>
+      <td>Sets the commanded joint positions as (name, angle) pairs.</td></tr>
+  <tr><td>\ref controller::HeadPanTiltControllerNode::getJointCmd "head_controller/get_command_array"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_GetJointCmd::GetJointCmd "GetJointCmd"</td>
+      <td>Gets the commanded joint positions as (name, angle) pairs. </td></tr>
+  <tr><td>\ref controller::HeadPanTiltControllerNode::trackPoint "head_controller/track_point"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_TrackPoint::TrackPoint "TrackPoint"</td>
+      <td>Sets the command position given (x, y, z, frame). </td></tr>
+  </table><br>
+  
+  - Laser Scanner Controller
+  <table border="1">
+  <tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
+  <tr><td>\ref controller::LaserScannerControllerNode::setCommand "tilt_laser_controller/set_command"</td>
+      <td>Double</td>
+      <td>Sets the commanded joint position. </td></tr>
+  <tr><td>\ref controller::LaserScannerControllerNode::getCommand "tilt_laser_controller/get_command"</td>
+      <td>Double</td>
+      <td>Gets the commanded joint position. </td></tr>
+  <tr><td>\ref controller::LaserScannerControllerNode::setProfile "tilt_laser_controller/track_point"</td>
+      <td>\ref pr2_mechanism_controllers::srv::_SetProfile::SetProfile "SetProfile"</td>
+      <td>Sets the sweep profile given (profile, period, amplitude, offset). </td></tr>
+  </table><br>
+     
+ 
+@todo rename base_controller/get_actual to base_controller/get_command.
+     update documentation for arm position controller services.
+
 **/
 
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-11 01:22:52 UTC (rev 5284)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-11 01:29:51 UTC (rev 5285)
@@ -185,7 +185,7 @@
      * \param req (names, positions)
      * \param resp (names, positions)
      */
-    bool setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+    bool setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
                     pr2_mechanism_controllers::SetJointCmd::response &resp);
 
     /*!

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-11 01:22:52 UTC (rev 5284)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-11 01:29:51 UTC (rev 5285)
@@ -165,7 +165,7 @@
 HeadPanTiltControllerNode::~HeadPanTiltControllerNode()
 {
   node->unadvertise_service(service_prefix + "/set_command_array");
-  node->unadvertise_service(service_prefix + "/get_command");
+  node->unadvertise_service(service_prefix + "/get_command_array");
 
   delete c_;
 }
@@ -184,8 +184,8 @@
   // Parses subcontroller configuration
   if(c_->initXml(robot, config))
   {
-    node->advertise_service(service_prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointSrv, this);
-    node->advertise_service(service_prefix + "/get_command", &HeadPanTiltControllerNode::getJointCmd, this);
+    node->advertise_service(service_prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointCmd, this);
+    node->advertise_service(service_prefix + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
     node->advertise_service(service_prefix + "/track_point", &HeadPanTiltControllerNode::trackPoint, this);
 
     return true;
@@ -193,7 +193,7 @@
   return false;
 }
 
-bool HeadPanTiltControllerNode::setJointSrv(pr2_mechanism_controllers::SetJointCmd::request &req,
+bool HeadPanTiltControllerNode::setJointCmd(pr2_mechanism_controllers::SetJointCmd::request &req,
                                    pr2_mechanism_controllers::SetJointCmd::response &resp)
 {
   std::vector<double> pos;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5322
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5322&view=rev
Author:   stuglaser
Date:     2008-10-14 02:33:16 +0000 (Tue, 14 Oct 2008)

Log Message:
-----------
Created the CasterCalibrationController

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-14 02:21:38 UTC (rev 5321)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-10-14 02:33:16 UTC (rev 5322)
@@ -13,7 +13,9 @@
   src/arm_velocity_controller.cpp
   src/arm_dynamics_controller.cpp
   src/head_pan_tilt_controller.cpp
-  src/caster_controller.cpp)
+  src/caster_controller.cpp
+  src/caster_calibration_controller.cpp
+  )
 
 rospack_add_executable(testBaseController test/test_base_controller.cpp)
 target_link_libraries(testBaseController pr2_mechanism_controllers)

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h	2008-10-14 02:33:16 UTC (rev 5322)
@@ -0,0 +1,99 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Stuart Glaser
+ */
+
+#ifndef CASTER_CALIBRATION_CONTROLLER_H
+#define CASTER_CALIBRATION_CONTROLLER_H
+
+#include "pr2_mechanism_controllers/caster_controller.h"
+#include <robot_mechanism_controllers/CalibrateJoint.h>
+
+namespace controller {
+
+class CasterCalibrationController : public Controller
+{
+public:
+  CasterCalibrationController();
+  ~CasterCalibrationController();
+
+  virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+  virtual void update();
+
+  bool calibrated() { return state_ == STOPPED; }
+  void beginCalibration()
+  {
+    if (state_ == INITIALIZED)
+      state_ = BEGINNING;
+  }
+
+protected:
+
+  enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+  int state_;
+
+  double search_velocity_;
+  bool original_switch_state_;
+
+  Actuator *actuator_;
+  mechanism::JointState *joint_;
+  mechanism::Transmission *transmission_;
+
+  controller::CasterController cc_;
+};
+
+
+class CasterCalibrationControllerNode : public Controller
+{
+public:
+  CasterCalibrationControllerNode();
+  ~CasterCalibrationControllerNode();
+
+  void update();
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req,
+                        robot_mechanism_controllers::CalibrateJoint::response &resp);
+
+private:
+  CasterCalibrationController c_;
+  AdvertisedServiceGuard guard_calibrate_;
+};
+
+}
+
+#endif

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp	2008-10-14 02:33:16 UTC (rev 5322)
@@ -0,0 +1,207 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Stuart Glaser
+ */
+
+#include "pr2_mechanism_controllers/caster_calibration_controller.h"
+
+namespace controller {
+
+CasterCalibrationController::CasterCalibrationController()
+  : state_(INITIALIZED)
+{
+}
+
+CasterCalibrationController::~CasterCalibrationController()
+{
+}
+
+bool CasterCalibrationController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  // This method is gross and ugly and should change (and the xml
+  // config format along with it)
+  assert(robot);
+  assert(config);
+
+  TiXmlElement *cal = config->FirstChildElement("calibrate");
+  if (!cal)
+  {
+    std::cerr<<"CasterCalibrationController was not given calibration parameters"<<std::endl;
+    return false;
+  }
+
+  if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
+  {
+    std::cerr<<"Velocity value was not specified\n";
+    return false;
+  }
+
+  const char *joint_name = cal->Attribute("joint");
+  joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+  if (!joint_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationController could not find joint \"%s\"\n",
+            joint_name);
+    return false;
+  }
+
+  const char *actuator_name = cal->Attribute("actuator");
+  actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+  if (!actuator_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationController could not find actuator \"%s\"\n",
+            actuator_name);
+    return false;
+  }
+
+  const char *transmission_name = cal->Attribute("transmission");
+  transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+  if (!transmission_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationController could not find transmission \"%s\"\n",
+            transmission_name);
+    return false;
+  }
+
+  control_toolbox::Pid pid;
+  TiXmlElement *pid_el = config->FirstChildElement("pid");
+  if (!pid_el)
+  {
+    fprintf(stderr, "Error: CasterCalibrationController was not given a pid element.\n");
+    return false;
+  }
+  if (!pid.initXml(pid_el))
+    return false;
+
+  if (!cc_.initXml(robot, config))
+    return false;
+
+  return true;
+}
+
+void CasterCalibrationController::update()
+{
+  assert(joint_);
+  assert(actuator_);
+
+  switch(state_)
+  {
+  case INITIALIZED:
+    cc_.steer_velocity_ = 0.0;
+    cc_.drive_velocity_ = 0.0;
+    break;
+  case BEGINNING:
+    original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+    cc_.steer_velocity_ = (original_switch_state_ ? search_velocity_ : -search_velocity_);
+    state_ = MOVING;
+    break;
+  case MOVING: {
+    bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+    if (switch_state_ != original_switch_state_)
+    {
+      std::vector<Actuator*> fake_a;
+      std::vector<mechanism::JointState*> fake_j;
+      fake_a.push_back(new Actuator);
+      fake_j.push_back(new mechanism::JointState);
+
+      // Where was the joint when the optical switch triggered?
+      if (original_switch_state_ = true)
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_high_transition_;
+      else
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_low_transition_;
+      transmission_->propagatePosition(fake_a, fake_j);
+
+      // What is the actuator position at the joint's zero?
+      fake_j[0]->position_ = fake_j[0]->position_ - joint_->joint_->reference_position_;
+      transmission_->propagatePositionBackwards(fake_j, fake_a);
+
+      actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+
+      state_ = STOPPED;
+    }
+    break;
+  }
+  case STOPPED:
+    cc_.steer_velocity_ = 0.0;
+    break;
+  }
+
+  cc_.update();
+}
+
+
+ROS_REGISTER_CONTROLLER(CasterCalibrationControllerNode)
+
+CasterCalibrationControllerNode::CasterCalibrationControllerNode()
+{
+}
+
+CasterCalibrationControllerNode::~CasterCalibrationControllerNode()
+{
+}
+
+void CasterCalibrationControllerNode::update()
+{
+  c_.update();
+}
+
+
+  bool CasterCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
+{
+  c_.beginCalibration();
+  ros::Duration d=ros::Duration(0,1000000);
+  while(!c_.calibrated())
+    d.sleep();
+  resp.offset = 0;
+  return true;
+}
+
+bool CasterCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  ros::node *node = ros::node::instance();
+
+  std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
+  if (topic == "")
+  {
+    fprintf(stderr, "No topic given to CasterCalibrationController\n");
+    return false;
+  }
+
+  if (!c_.initXml(robot, config))
+    return false;
+  node->advertise_service(topic + "/calibrate", &CasterCalibrationControllerNode::calibrateCommand, this);
+  guard_calibrate_.set(topic + "/calibrate");
+
+  return true;
+}
+
+
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5446
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5446&view=rev
Author:   sachinchitta
Date:     2008-10-16 21:41:26 +0000 (Thu, 16 Oct 2008)

Log Message:
-----------
Odometry yaw angle is now normalized to be between -pi to pi

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-16 21:36:46 UTC (rev 5445)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-16 21:41:26 UTC (rev 5446)
@@ -100,7 +100,7 @@
   cmd_vel_t_.y = clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
   cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
-  std::cout << "BaseController:: command received: " << cmd_vel_t_;
+  std::cout << "BaseController:: command received: " << cmd_vel;
   std::cout << "BaseController:: command current: " << cmd_vel_;
   std::cout << "Base Odometry: Velocity " << base_odom_velocity_;
   std::cout << "Base Odometry: Position " << base_odom_position_;
@@ -114,538 +114,538 @@
 
 libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
 {
-   libTF::Vector result;
-   result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
-   result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
-   result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
-   return result;
+  libTF::Vector result;
+  result.x = start.x + clamp(end.x - start.x,-max_rate.x*dT,max_rate.x*dT);
+  result.y = start.y + clamp(end.y - start.y,-max_rate.y*dT,max_rate.y*dT);
+  result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
+  return result;
 }
 
 libTF::Vector BaseController::getCommand()// Return the current position command
 {
-   libTF::Vector cmd_vel;
-   pthread_mutex_lock(&base_controller_lock_);
-   cmd_vel.x = cmd_vel_.x;
-   cmd_vel.y = cmd_vel_.y;
-   cmd_vel.z = cmd_vel_.z;
-   pthread_mutex_unlock(&base_controller_lock_);
-   return cmd_vel;
+  libTF::Vector cmd_vel;
+  pthread_mutex_lock(&base_controller_lock_);
+  cmd_vel.x = cmd_vel_.x;
+  cmd_vel.y = cmd_vel_.y;
+  cmd_vel.z = cmd_vel_.z;
+  pthread_mutex_unlock(&base_controller_lock_);
+  return cmd_vel;
 }
 
 void BaseController::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot_state)
 {
-   std::vector<JointControlParam>::iterator jcp_iter;
-   robot_desc::URDF::Link *link;
-   std::string joint_name;
+  std::vector<JointControlParam>::iterator jcp_iter;
+  robot_desc::URDF::Link *link;
+  std::string joint_name;
 
-   std::string xml_content;
-   (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+  std::string xml_content;
+  (ros::g_node)->get_param("robotdesc/pr2",xml_content);
 
-   // wait for robotdesc/pr2 on param server
-   while(!urdf_model_.loadString(xml_content.c_str()))
-   {
-      std::cout << "WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar." << std::endl;
-      (ros::g_node)->get_param("robotdesc/pr2",xml_content);
-      usleep(100000);
-   }
+  // wait for robotdesc/pr2 on param server
+  while(!urdf_model_.loadString(xml_content.c_str()))
+  {
+    std::cout << "WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar." << std::endl;
+    (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+    usleep(100000);
+  }
 
-   for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
-   {
-      joint_name = jcp_iter->joint_name;
-      link = urdf_model_.getJointLink(joint_name);
+  for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
+  {
+    joint_name = jcp_iter->joint_name;
+    link = urdf_model_.getJointLink(joint_name);
 
-      BaseParam base_object;
-      base_object.pos_.x = link->xyz[0];
-      base_object.pos_.y = link->xyz[1];
-      base_object.pos_.z = link->xyz[2];
-      base_object.name_ = joint_name;
-      base_object.parent_ = NULL;
-      base_object.joint_state_ = robot_state->getJointState(joint_name);
-      if (base_object.joint_state_==NULL)
-         std::cout << " unsuccessful getting joint state for " << joint_name << std::endl;
+    BaseParam base_object;
+    base_object.pos_.x = link->xyz[0];
+    base_object.pos_.y = link->xyz[1];
+    base_object.pos_.z = link->xyz[2];
+    base_object.name_ = joint_name;
+    base_object.parent_ = NULL;
+    base_object.joint_state_ = robot_state->getJointState(joint_name);
+    if (base_object.joint_state_==NULL)
+      std::cout << " unsuccessful getting joint state for " << joint_name << std::endl;
 
-      base_object.controller_.init(robot_state, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+    base_object.controller_.init(robot_state, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
 
-      if(joint_name.find("caster") != string::npos)
-      {
+    if(joint_name.find("caster") != string::npos)
+    {
 //         std::cout << " assigning casters " << joint_name << std::endl;
-         base_object.local_id_ = num_casters_;
-         base_casters_.push_back(base_object);
-         steer_angle_actual_.push_back(0);
-         steer_velocity_desired_.push_back(0);
-         num_casters_++;
-//         cout << "base_casters" << "::  " << base_object;
-      }
+      base_object.local_id_ = num_casters_;
+      base_casters_.push_back(base_object);
+      steer_angle_actual_.push_back(0);
+      steer_velocity_desired_.push_back(0);
+      num_casters_++;
+      cout << "base_casters" << "::  " << base_object;
+    }
 
-      if(joint_name.find("wheel") != string::npos)
-      {
+    if(joint_name.find("wheel") != string::npos)
+    {
 //         std::cout << " assigning wheels " << joint_name << std::endl;
-         base_object.local_id_ = num_wheels_;
-         if(joint_name.find("_r_") != string::npos)
-            base_object.direction_multiplier_ = 1;
+      base_object.local_id_ = num_wheels_;
+      if(joint_name.find("_r_") != string::npos)
+        base_object.direction_multiplier_ = 1;
 
-         base_wheels_.push_back(base_object);
-         wheel_speed_actual_.push_back(0);
-         libTF::Vector *v=new libTF::Vector();
-         base_wheels_position_.push_back(*v);
-         wheel_speed_cmd_.push_back(0);      
-         num_wheels_++;
-      }
-   }
+      base_wheels_.push_back(base_object);
+      wheel_speed_actual_.push_back(0);
+      libTF::Vector *v=new libTF::Vector();
+      base_wheels_position_.push_back(*v);
+      wheel_speed_cmd_.push_back(0);      
+      num_wheels_++;
+    }
+  }
 
-   for(int i =0; i < num_wheels_; i++)
-   {
-      link = urdf_model_.getJointLink(base_wheels_[i].name_);
-      std::string parent_name = link->parent->joint->name;
+  for(int i =0; i < num_wheels_; i++)
+  {
+    link = urdf_model_.getJointLink(base_wheels_[i].name_);
+    std::string parent_name = link->parent->joint->name;
 //    cout << "parent_name from urdf:: " << parent_name << endl;
-      for(int j =0; j < num_casters_; j++)
+    for(int j =0; j < num_casters_; j++)
+    {
+      if(parent_name == base_casters_[j].name_)
       {
-         if(parent_name == base_casters_[j].name_)
-         {
 //        cout <<  "base_casters matched name  ::" << base_casters_[j].name_ << endl;
-            base_wheels_[i].parent_ = &base_casters_[j];
-            break;
-         }
+        base_wheels_[i].parent_ = &base_casters_[j];
+        break;
       }
+    }
 //      cout << "parent assigned name ::" << base_wheels_[i].parent_->name_ << endl << endl;
 //      cout << "base_wheels" << endl << base_wheels_[i];
-   }
+  }
 
-   if(num_wheels_ > 0)
-   {
-      link = urdf_model_.getJointLink(base_wheels_[0].name_);
-      robot_desc::URDF::Link::Geometry::Cylinder *wheel_geom = dynamic_cast<robot_desc::URDF::Link::Geometry::Cylinder*> (link->collision->geometry->shape);
-      wheel_radius_ = wheel_geom->radius;
-   }
-   else
-   {
-      wheel_radius_ = DEFAULT_WHEEL_RADIUS;
-   }
-   robot_state_ = robot_state;
+  if(num_wheels_ > 0)
+  {
+    link = urdf_model_.getJointLink(base_wheels_[0].name_);
+    robot_desc::URDF::Link::Geometry::Cylinder *wheel_geom = dynamic_cast<robot_desc::URDF::Link::Geometry::Cylinder*> (link->collision->geometry->shape);
+    wheel_radius_ = wheel_geom->radius;
+  }
+  else
+  {
+    wheel_radius_ = DEFAULT_WHEEL_RADIUS;
+  }
+  robot_state_ = robot_state;
 
-   last_time_ = robot_state_->hw_->current_time_;
+  last_time_ = robot_state_->hw_->current_time_;
 
-   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 }
 
 bool BaseController::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
 
-   std::cout << " base controller name: " << config->Attribute("name") << std::endl;
-   TiXmlElement *elt = config->FirstChildElement("controller");
-   std::vector<JointControlParam> jcp_vec;
-   JointControlParam jcp;
-   while (elt){
-      TiXmlElement *jnt = elt->FirstChildElement("joint");
-      jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
-      jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
-      jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
-      jcp.windup = atof(jnt->FirstChildElement("pid")->Attribute("iClamp"));
-      jcp.control_type = (std::string) elt->Attribute("type");
-      jcp.joint_name = jnt->Attribute("name");
-      jcp_vec.push_back(jcp);
+  std::cout << " base controller name: " << config->Attribute("name") << std::endl;
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  std::vector<JointControlParam> jcp_vec;
+  JointControlParam jcp;
+  while (elt){
+    TiXmlElement *jnt = elt->FirstChildElement("joint");
+    jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
+    jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
+    jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
+    jcp.windup = atof(jnt->FirstChildElement("pid")->Attribute("iClamp"));
+    jcp.control_type = (std::string) elt->Attribute("type");
+    jcp.joint_name = jnt->Attribute("name");
+    jcp_vec.push_back(jcp);
 
-      std::cout << "name:" << jcp.joint_name << std::endl;
-      std::cout << "controller type:" << jcp.control_type << std::endl;
-      std::cout << "sub controller: " << elt->Attribute("name") << std::endl;
+    std::cout << "name:" << jcp.joint_name << std::endl;
+    std::cout << "controller type:" << jcp.control_type << std::endl;
+    std::cout << "sub controller: " << elt->Attribute("name") << std::endl;
 
-      elt = elt->NextSiblingElement("controller");
-   }
+    elt = elt->NextSiblingElement("controller");
+  }
 
-   addParamToMap("kp_speed",&kp_speed_);
-   addParamToMap("kp_caster_steer",&caster_steer_vel_gain_);
-   addParamToMap("timeout",&timeout_);
-   addParamToMap("max_x_dot",&(max_vel_.x));
-   addParamToMap("max_y_dot",&(max_vel_.y));
-   addParamToMap("max_yaw_dot",&(max_vel_.z));
-   addParamToMap("max_x_accel",&(max_accel_.x));
-   addParamToMap("max_y_accel",&(max_accel_.y));
-   addParamToMap("max_yaw_accel",&(max_accel_.z));
+  addParamToMap("kp_speed",&kp_speed_);
+  addParamToMap("kp_caster_steer",&caster_steer_vel_gain_);
+  addParamToMap("timeout",&timeout_);
+  addParamToMap("max_x_dot",&(max_vel_.x));
+  addParamToMap("max_y_dot",&(max_vel_.y));
+  addParamToMap("max_yaw_dot",&(max_vel_.z));
+  addParamToMap("max_x_accel",&(max_accel_.x));
+  addParamToMap("max_y_accel",&(max_accel_.y));
+  addParamToMap("max_yaw_accel",&(max_accel_.z));
 
-   elt = config->FirstChildElement("map");
+  elt = config->FirstChildElement("map");
 
-   while(elt)
-   {
-      if(elt->Attribute("name") == std::string("velocity_control"))
+  while(elt)
+  {
+    if(elt->Attribute("name") == std::string("velocity_control"))
+    {
+      TiXmlElement *elt_key = elt->FirstChildElement("elem");
+      while(elt_key)
       {
-         TiXmlElement *elt_key = elt->FirstChildElement("elem");
-         while(elt_key)
-         {
-            *(param_map_[elt_key->Attribute("key")]) = atof(elt_key->GetText());
-            elt_key = elt_key->NextSiblingElement("elem");
-         }
+        *(param_map_[elt_key->Attribute("key")]) = atof(elt_key->GetText());
+        elt_key = elt_key->NextSiblingElement("elem");
       }
-      std::cout << "*************************************" << std::endl;
-      elt = config->NextSiblingElement("map");
-   }
+    }
+    std::cout << "*************************************" << std::endl;
+    elt = config->NextSiblingElement("map");
+  }
 
-   cout << "kp_speed  " << kp_speed_ << endl;
-   cout << "kp_caster_steer  " << caster_steer_vel_gain_ << endl;
-   cout << "timeout  " << timeout_ << endl;
-   cout << "max_x_dot  " << (max_vel_.x) << endl;
-   cout << "max_y_dot  " << (max_vel_.y) << endl;
-   cout << "max_yaw_dot  " << (max_vel_.z) << endl;
-   cout << "max_x_accel  " << (max_accel_.x) << endl;
-   cout << "max_y_accel  " << (max_accel_.y) << endl;
-   cout << "max_yaw_accel  " << (max_accel_.z) << endl;
+  cout << "kp_speed  " << kp_speed_ << endl;
+  cout << "kp_caster_steer  " << caster_steer_vel_gain_ << endl;
+  cout << "timeout  " << timeout_ << endl;
+  cout << "max_x_dot  " << (max_vel_.x) << endl;
+  cout << "max_y_dot  " << (max_vel_.y) << endl;
+  cout << "max_yaw_dot  " << (max_vel_.z) << endl;
+  cout << "max_x_accel  " << (max_accel_.x) << endl;
+  cout << "max_y_accel  " << (max_accel_.y) << endl;
+  cout << "max_yaw_accel  " << (max_accel_.z) << endl;
 
-   init(jcp_vec,robot_state);
-   return true;
+  init(jcp_vec,robot_state);
+  return true;
 }
 
 void BaseController::addParamToMap(std::string key, double *value)
 {
-   param_map_[key] = value;
+  param_map_[key] = value;
 }
 
 void BaseController::getJointValues()
 {
-   for(int i=0; i < num_casters_; i++)
-      steer_angle_actual_[i] = base_casters_[i].joint_state_->position_;
+  for(int i=0; i < num_casters_; i++)
+    steer_angle_actual_[i] = base_casters_[i].joint_state_->position_;
 
-   for(int i=0; i < num_wheels_; i++)
-      wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
+  for(int i=0; i < num_wheels_; i++)
+    wheel_speed_actual_[i] = base_wheels_[i].controller_.getMeasuredVelocity();
 }
 
 void BaseController::computeWheelPositions()
 {
-   libTF::Vector res1;
-   double steer_angle;
-   for(int i=0; i < num_wheels_; i++)
-   {
-      steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-      res1 = base_wheels_[i].pos_.rot2D(steer_angle);
-      res1 += base_wheels_[i].parent_->pos_;
-      base_wheels_position_[i] = res1;
-   }
+  libTF::Vector res1;
+  double steer_angle;
+  for(int i=0; i < num_wheels_; i++)
+  {
+    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+    res1 = base_wheels_[i].pos_.rot2D(steer_angle);
+    res1 += base_wheels_[i].parent_->pos_;
+    base_wheels_position_[i] = res1;
+  }
 }
 
 void BaseController::update()
 {
-   double current_time = robot_state_->hw_->current_time_;
-   double dT = std::min<double>(current_time - last_time_,MAX_DT_);
+  double current_time = robot_state_->hw_->current_time_;
+  double dT = std::min<double>(current_time - last_time_,MAX_DT_);
 
-   if(new_cmd_available_)
-   {
-      if(pthread_mutex_trylock(&base_controller_lock_)==0)
-      {
-         desired_vel_.x = cmd_vel_t_.x;
-         desired_vel_.y = cmd_vel_t_.y;
-         desired_vel_.z = cmd_vel_t_.z;
-         new_cmd_available_ = false;
-         pthread_mutex_unlock(&base_controller_lock_);
-      }
-   }
+  if(new_cmd_available_)
+  {
+    if(pthread_mutex_trylock(&base_controller_lock_)==0)
+    {
+      desired_vel_.x = cmd_vel_t_.x;
+      desired_vel_.y = cmd_vel_t_.y;
+      desired_vel_.z = cmd_vel_t_.z;
+      new_cmd_available_ = false;
+      pthread_mutex_unlock(&base_controller_lock_);
+    }
+  }
 
-   if((current_time - cmd_received_timestamp_) > timeout_)
-   {
-      cmd_vel_.x = 0;
-      cmd_vel_.y = 0;
-      cmd_vel_.z = 0;
-   }
-   else
-      cmd_vel_ = interpolateCommand(cmd_vel_,desired_vel_,max_accel_,dT);
+  if((current_time - cmd_received_timestamp_) > timeout_)
+  {
+    cmd_vel_.x = 0;
+    cmd_vel_.y = 0;
+    cmd_vel_.z = 0;
+  }
+  else
+    cmd_vel_ = interpolateCommand(cmd_vel_,desired_vel_,max_accel_,dT);
 
 
-   getJointValues();
+  getJointValues();
 
-   computeJointCommands();
+  computeJointCommands();
 
-   setJointCommands();
+  setJointCommands();
 
-   computeOdometry(current_time);
+  computeOdometry(current_time);
 
-   updateJointControllers();
+  updateJointControllers();
 
-   last_time_ = current_time;
+  last_time_ = current_time;
 }
 
 
 void BaseController::computeJointCommands()
 {
-   computeWheelPositions();
+  computeWheelPositions();
 
-   computeDesiredCasterSteer();
+  computeDesiredCasterSteer();
 
-   computeDesiredWheelSpeeds();  
+  computeDesiredWheelSpeeds();  
 }
 
 void BaseController::setJointCommands()
 {
-   setDesiredCasterSteer();
+  setDesiredCasterSteer();
 
-   setDesiredWheelSpeeds();
+  setDesiredWheelSpeeds();
 }
 
 void BaseController::computeDesiredCasterSteer()
 {
-   libTF::Vector result;
+  libTF::Vector result;
 
-   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
-   double error_steer(0.0),error_steer_m_pi(0.0);
+  double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
+  double error_steer(0.0),error_steer_m_pi(0.0);
 
-   for(int i=0; i < num_casters_; i++)
-   {
-      result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
+  for(int i=0; i < num_casters_; i++)
+  {
+    result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
 
-      steer_angle_desired = atan2(result.y,result.x);
-      steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
+    steer_angle_desired = atan2(result.y,result.x);
+    steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
 
-      error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
-      error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
+    error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+    error_steer_m_pi = shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
 
-      if(fabs(error_steer_m_pi) < fabs(error_steer))
-      {
-         error_steer = error_steer_m_pi;
-         steer_angle_desired = steer_angle_desired_m_pi;
-      }
-      steer_velocity_desired_[i] =  -kp_speed_*error_steer;
-   }
+    if(fabs(error_steer_m_pi) < fabs(error_steer))
+    {
+      error_steer = error_steer_m_pi;
+      steer_angle_desired = steer_angle_desired_m_pi;
+    }
+    steer_velocity_desired_[i] =  -kp_speed_*error_steer;
+  }
 }
 
 void BaseController::setDesiredCasterSteer()
 {
-   for(int i=0; i < num_casters_; i++)
-      base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
+  for(int i=0; i < num_casters_; i++)
+    base_casters_[i].controller_.setCommand(steer_velocity_desired_[i]);
 }
 
 void BaseController::computeDesiredWheelSpeeds()
 {
-   libTF::Vector wheel_point_velocity;
-   libTF::Vector wheel_point_velocity_projected;
-   libTF::Vector wheel_caster_steer_component;
-   libTF::Vector caster_2d_velocity;
+  libTF::Vector wheel_point_velocity;
+  libTF::Vector wheel_point_velocity_projected;
+  libTF::Vector wheel_caster_steer_component;
+  libTF::Vector caster_2d_velocity;
 
-   caster_2d_velocity.x = 0;
-   caster_2d_velocity.y = 0;
-   caster_2d_velocity.z = 0;
+  caster_2d_velocity.x = 0;
+  caster_2d_velocity.y = 0;
+  caster_2d_velocity.z = 0;
 
-   double steer_angle_actual = 0;
-   for(int i=0; i < (int) num_wheels_; i++)
-   {
-      caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
-      steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
-      wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
+  double steer_angle_actual = 0;
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+    caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
+    steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
+    wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
 
-      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-      wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-      wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
-   }
+    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+    wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
+    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+  }
 }
 
 void BaseController::setDesiredWheelSpeeds()
 {
-   for(int i=0; i < (int) num_wheels_; i++)
-   {
-      base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]);
-   }
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+    base_wheels_[i].controller_.setCommand(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]);
+  }
 }
 
 
 void BaseController::updateJointControllers()
 {
-   for(int i=0; i < num_wheels_; i++)
-      base_wheels_[i].controller_.update();
-   for(int i=0; i < num_casters_; i++)
-      base_casters_[i].controller_.update();
+  for(int i=0; i < num_wheels_; i++)
+    base_wheels_[i].controller_.update();
+  for(int i=0; i < num_casters_; i++)
+    base_casters_[i].controller_.update();
 }
 
 void BaseController::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
 {
-   odom_msg_.header.frame_id   = "odom";
-   odom_msg_.header.stamp.sec  = (unsigned long)floor(robot_state_->hw_->current_time_);
-   odom_msg_.header.stamp.nsec = (unsigned long)floor(  1e9 * (  robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
+  odom_msg_.header.frame_id   = "odom";
+  odom_msg_.header.stamp.sec  = (unsigned long)floor(robot_state_->hw_->current_time_);
+  odom_msg_.header.stamp.nsec = (unsigned long)floor(  1e9 * (  robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
 
-   odom_msg_.pos.x  = base_odom_position_.x;
-   odom_msg_.pos.y  = base_odom_position_.y;
-   odom_msg_.pos.th = base_odom_position_.z;
+  odom_msg_.pos.x  = base_odom_position_.x;
+  odom_msg_.pos.y  = base_odom_position_.y;
+  odom_msg_.pos.th = math_utils::normalize_angle(base_odom_position_.z);
 
-   odom_msg_.vel.x  = base_odom_velocity_.x;
-   odom_msg_.vel.y  = base_odom_velocity_.y;
-   odom_msg_.vel.th = base_odom_velocity_.z;
+  odom_msg_.vel.x  = base_odom_velocity_.x;
+  odom_msg_.vel.y  = base_odom_velocity_.y;
+  odom_msg_.vel.th = base_odom_velocity_.z;
 }
 
 
 void BaseController::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
 {
-   x = base_odom_position_.x;
-   y = base_odom_position_.y;
-   w = base_odom_position_.z;
+  x = base_odom_position_.x;
+  y = base_odom_position_.y;
+  w = base_odom_position_.z;
 
-   vx = base_odom_velocity_.x;
-   vy = base_odom_velocity_.y;
-   vw = base_odom_velocity_.z;
+  vx = base_odom_velocity_.x;
+  vy = base_odom_velocity_.y;
+  vw = base_odom_velocity_.z;
 }
 
 
 Vector BaseController::computePointVelocity2D(const Vector& pos, const Vector& vel)
 {
-   Vector result;
+  Vector result;
 
-   result.x = vel.x - pos.y * vel.z;
-   result.y = vel.y + pos.x * vel.z;
-   result.z = 0;
+  result.x = vel.x - pos.y * vel.z;
+  result.y = vel.y + pos.x * vel.z;
+  result.z = 0;
 
-   return result;
+  return result;
 }
 
 void BaseController::computeOdometry(double time)
 {
-   double dt = time-last_time_;
+  double dt = time-last_time_;
 
-   computeBaseVelocity();
+  computeBaseVelocity();
 
-   libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
+  libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
 
-   base_odom_delta.z = base_odom_velocity_.z * dt;
-   base_odom_position_ += base_odom_delta;
+  base_odom_delta.z = base_odom_velocity_.z * dt;
+  base_odom_position_ += base_odom_delta;
 }
 
 void BaseController::computeBaseVelocity()
 {
-   Matrix A(2*num_wheels_,1);
-   Matrix C(2*num_wheels_,3);
-   Matrix D(3,1);
-   double steer_angle;
-   double wheel_speed;
-   libTF::Vector caster_2d_velocity;
-   libTF::Vector wheel_caster_steer_component;
+  Matrix A(2*num_wheels_,1);
+  Matrix C(2*num_wheels_,3);
+  Matrix D(3,1);
+  double steer_angle;
+  double wheel_speed;
+  libTF::Vector caster_2d_velocity;
+  libTF::Vector wheel_caster_steer_component;
 
-   caster_2d_velocity.x = 0;
-   caster_2d_velocity.y = 0;
+  caster_2d_velocity.x = 0;
+  caster_2d_velocity.y = 0;
 
-   for(int i = 0; i < num_wheels_; i++) {
-      caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
-      wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-      wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_; 
+  for(int i = 0; i < num_wheels_; i++) {
+    caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
+    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_; 
 
-      steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-      A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
-      A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
-   }
+    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
+    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
+  }
 
-   for(int i = 0; i < num_wheels_; i++) {
-      C.element(i*2, 0)   = 1;
-      C.element(i*2, 1)   = 0;
-      C.element(i*2, 2)   = -base_wheels_position_[i].y;
-      C.element(i*2+1, 0) = 0;
-      C.element(i*2+1, 1) = 1;
-      C.element(i*2+1, 2) =  base_wheels_position_[i].x;
-   }
-   //   D = pseudoInverse(C)*A;
+  for(int i = 0; i < num_wheels_; i++) {
+    C.element(i*2, 0)   = 1;
+    C.element(i*2, 1)   = 0;
+    C.element(i*2, 2)   = -base_wheels_position_[i].y;
+    C.element(i*2+1, 0) = 0;
+    C.element(i*2+1, 1) = 1;
+    C.element(i*2+1, 2) =  base_wheels_position_[i].x;
+  }
+  //   D = pseudoInverse(C)*A;
   D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
-   base_odom_velocity_.x = (double)D.element(0,0);
-   base_odom_velocity_.y = (double)D.element(1,0);
-   base_odom_velocity_.z = (double)D.element(2,0);
+  base_odom_velocity_.x = (double)D.element(0,0);
+  base_odom_velocity_.y = (double)D.element(1,0);
+  base_odom_velocity_.z = (double)D.element(2,0);
 }
 
 Matrix BaseController::pseudoInverse(const Matrix M)
 {
-   Matrix result;
-   //int rows = this->rows();
-   //int cols = this->columns();
-   // calculate SVD decomposition
-   Matrix U,V;
-   DiagonalMatrix D;
-   NEWMAT::SVD(M,D,U,V, true, true);
-   Matrix Dinv = D.i();
-   result = V * Dinv * U.t();
-   return result;
+  Matrix result;
+  //int rows = this->rows();
+  //int cols = this->columns();
+  // calculate SVD decomposition
+  Matrix U,V;
+  DiagonalMatrix D;
+  NEWMAT::SVD(M,D,U,V, true, true);
+  Matrix Dinv = D.i();
+  result = V * Dinv * U.t();
+  return result;
 }
 
 std::ostream & controller::operator<<(std::ostream& mystream, const controller::BaseParam &bp)
 {
-   mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
-   return mystream;
+  mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
+  return mystream;
 };
 
 NEWMAT::Matrix BaseController::iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter)
 {
-   NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
-   NEWMAT::Matrix weight_matrix(2*num_wheels_,2*num_wheels_);
-   NEWMAT::Matrix a_fit(2*num_wheels_,3);
-   NEWMAT::Matrix b_fit(2*num_wheels_,1);
-   NEWMAT::Matrix residual(2*num_wheels_,1);
-   NEWMAT::Matrix x_fit(3,1);
+  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+  NEWMAT::Matrix weight_matrix(2*num_wheels_,2*num_wheels_);
+  NEWMAT::Matrix a_fit(2*num_wheels_,3);
+  NEWMAT::Matrix b_fit(2*num_wheels_,1);
+  NEWMAT::Matrix residual(2*num_wheels_,1);
+  NEWMAT::Matrix x_fit(3,1);
 
-   weight_matrix = ident_matrix;
+  weight_matrix = ident_matrix;
 
-   for(int i=0; i < max_iter; i++)
-   {
-      a_fit = weight_matrix * A;
-      b_fit = weight_matrix * b;
-      x_fit = pseudoInverse(a_fit)*b_fit;
-      residual = b - A * x_fit;
+  for(int i=0; i < max_iter; i++)
+  {
+    a_fit = weight_matrix * A;
+    b_fit = weight_matrix * b;
+    x_fit = pseudoInverse(a_fit)*b_fit;
+    residual = b - A * x_fit;
 
-      for(int j=1; j <= num_wheels_; j++)
+    for(int j=1; j <= num_wheels_; j++)
+    {
+      int fw = 2*j-1;
+      int sw = fw+1;
+      if(fabs(residual(fw,1)) > fabs(residual(sw,1)))
       {
-         int fw = 2*j-1;
-         int sw = fw+1;
-         if(fabs(residual(fw,1)) > fabs(residual(sw,1)))
-         {
-            residual(fw,1) = fabs(residual(fw,1));
-            residual(sw,1) = residual(fw,1);
-         }
-         else
-         {
-            residual(fw,1) = fabs(residual(sw,1));
-            residual(sw,1) = residual(fw,1);
-         }
+        residual(fw,1) = fabs(residual(fw,1));
+        residual(sw,1) = residual(fw,1);
       }
-      weight_matrix = findWeightMatrix(residual,weight_type);
-   }
-   return x_fit;
+      else
+      {
+        residual(fw,1) = fabs(residual(sw,1));
+        residual(sw,1) = residual(fw,1);
+      }
+    }
+    weight_matrix = findWeightMatrix(residual,weight_type);
+  }
+  return x_fit;
 };
 
 NEWMAT::Matrix BaseController::findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type)
 {
-   NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
-   NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+  NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
+  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
 
-   w_fit = ident_matrix;
-   double epsilon = 0;
-   double g_sigma = 0.1;
+  w_fit = ident_matrix;
+  double epsilon = 0;
+  double g_sigma = 0.1;
 
-   if(weight_type == std::string("BubeLagan"))
-   {
-      for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
-      {
-         if(fabs(residual(i,1) > epsilon))
-            epsilon = fabs(residual(i,1));
-      }
-      epsilon = epsilon/100.0;
-   }
-   for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
-   {
-      if(weight_type == std::string("L1norm"))
-      {
-         w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
-         cout << residual(i,1) << endl;
-      }
-      else if(weight_type == std::string("fair"))
-      {
-         w_fit(i,i) = 1.0/(1 + fabs(residual(i,1)));
-      }
-      else if(weight_type == std::string("Cauchy"))
-      {
-         w_fit(i,i) = 1.0/(1 + residual(i,1)*residual(i,1));
-      }
-      else if(weight_type == std::string("BubeLangan"))
-      {
-         w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
-         cout << residual(i,1) << endl;
-      }
-      else if(weight_type == std::string("Gaussian"))
-      {
-         w_fit(i,i) = sqrt(exp(-pow(residual(i,1),2)/(2*g_sigma*g_sigma)));
-      }
-      else // default to fair
-      {
-         cout << "Default" << endl;
-         w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
-      }
-   }
-   return w_fit;
+  if(weight_type == std::string("BubeLagan"))
+  {
+    for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+    {
+      if(fabs(residual(i,1) > epsilon))
+        epsilon = fabs(residual(i,1));
+    }
+    epsilon = epsilon/100.0;
+  }
+  for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+  {
+    if(weight_type == std::string("L1norm"))
+    {
+      w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
+      cout << residual(i,1) << endl;
+    }
+    else if(weight_type == std::string("fair"))
+    {
+      w_fit(i,i) = 1.0/(1 + fabs(residual(i,1)));
+    }
+    else if(weight_type == std::string("Cauchy"))
+    {
+      w_fit(i,i) = 1.0/(1 + residual(i,1)*residual(i,1));
+    }
+    else if(weight_type == std::string("BubeLangan"))
+    {
+      w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
+      cout << residual(i,1) << endl;
+    }
+    else if(weight_type == std::string("Gaussian"))
+    {
+      w_fit(i,i) = sqrt(exp(-pow(residual(i,1),2)/(2*g_sigma*g_sigma)));
+    }
+    else // default to fair
+    {
+      cout << "Default" << endl;
+      w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
+    }
+  }
+  return w_fit;
 };
 
 
@@ -654,28 +654,28 @@
 
 ROS_REGISTER_CONTROLLER(BaseControllerNode)
 
-BaseControllerNode::BaseControllerNode()
+  BaseControllerNode::BaseControllerNode()
 {
-   c_ = new BaseController();
-   node = ros::node::instance();
-   last_time_message_sent_ = 0.0;
-   odom_publish_rate_ = 100.0;
-   odom_publish_delta_t_ = 1.0/odom_publish_rate_;
-   publisher_ = NULL;
-   transform_publisher_ = NULL;
+  c_ = new BaseController();
+  node = ros::node::instance();
+  last_time_message_sent_ = 0.0;
+  odom_publish_rate_ = 100.0;
+  odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+  publisher_ = NULL;
+  transform_publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
 {
-   node->unadvertise_service(service_prefix + "/set_command");
-   node->unadvertise_service(service_prefix + "/get_actual");
-   node->unsubscribe("cmd_vel");
+  node->unadvertise_service(service_prefix + "/set_command");
+  node->unadvertise_service(service_prefix + "/get_actual");
+  node->unsubscribe("cmd_vel");
 
-   publisher_->stop();
-   transform_publisher_->stop();
-   delete publisher_;
-   delete transform_publisher_;
-   delete c_;
+  publisher_->stop();
+  transform_publisher_->stop();
+  delete publisher_;
+  delete transform_publisher_;
+  delete c_;
 }
 
 void BaseControllerNode::update()
@@ -707,64 +707,64 @@
       out.z = 0;
       out.roll = 0;
       out.pitch = 0;
-      out.yaw = -yaw;
+      out.yaw = -math_utils::normalize_angle(yaw);
       transform_publisher_->unlockAndPublish() ;
     }
   }
 }
 
 bool BaseControllerNode::setCommand(
-   pr2_mechanism_controllers::SetBaseCommand::request &req,
-   pr2_mechanism_controllers::SetBaseCommand::response &resp)
+  pr2_mechanism_controllers::SetBaseCommand::request &req,
+  pr2_mechanism_controllers::SetBaseCommand::response &resp)
 {
-   libTF::Vector command;
-   command.x = req.x_vel;
-   command.y = req.y_vel;
-   command.z = req.theta_vel;
-   c_->setCommand(command);
-   command = c_->getCommand();
-   resp.x_vel = command.x;
-   resp.y_vel = command.y;
-   resp.theta_vel = command.z;
-   return true;
+  libTF::Vector command;
+  command.x = req.x_vel;
+  command.y = req.y_vel;
+  command.z = req.theta_vel;
+  c_->setCommand(command);
+  command = c_->getCommand();
+  resp.x_vel = command.x;
+  resp.y_vel = command.y;
+  resp.theta_vel = command.z;
+  return true;
 }
 
 void BaseControllerNode::setCommand(double vx, double vy, double vw)
 {
-   libTF::Vector command;
-   command.x = vx;
-   command.y = vy;
-   command.z = vw;
-   c_->setCommand(command);
-   std::cout << "BaseController:: odom_publish_rate: " << odom_publish_rate_ << endl;
+  libTF::Vector command;
+  command.x = vx;
+  command.y = vy;
+  command.z = vw;
+  c_->setCommand(command);
+  std::cout << "BaseController:: odom_publish_rate: " << odom_publish_rate_ << endl;
 }
 
 
 bool BaseControllerNode::getCommand(
-   pr2_mechanism_controllers::GetBaseCommand::request &req,
-   pr2_mechanism_controllers::GetBaseCommand::response &resp)
+  pr2_mechanism_controllers::GetBaseCommand::request &req,
+  pr2_mechanism_controllers::GetBaseCommand::response &resp)
 {
-   libTF::Vector command;
-   command = c_->getCommand();
-   resp.x_vel = command.x;
-   resp.y_vel = command.y;
-   resp.theta_vel = command.z;
+  libTF::Vector command;
+  command = c_->getCommand();
+  resp.x_vel = command.x;
+  resp.y_vel = command.y;
+  resp.theta_vel = command.z;
 
-   return true;
+  return true;
 }
 
 bool BaseControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
-   service_prefix = config->Attribute("name");
+  service_prefix = config->Attribute("name");
 
-   assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
+  assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
 
-   if(!c_->initXml(robot_state, config))
-      return false;
+  if(!c_->initXml(robot_state, config))
+    return false;
 
-   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
-   node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
-   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
+  node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
+  node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+  node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
 
  
   if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
@@ -781,19 +781,19 @@
   transform_publisher_->msg_.set_eulers_size(1);
 
   if(odom_publish_rate_ > 1e-5)
-     odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+    odom_publish_delta_t_ = 1.0/odom_publish_rate_;
 
-   return true;
+  return true;
 }
 
 void BaseControllerNode::CmdBaseVelReceived()
 {
-   this->ros_lock_.lock();
-   this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
-   this->ros_lock_.unlock();
+  this->ros_lock_.lock();
+  this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+  this->ros_lock_.unlock();
 }
 
 void BaseControllerNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
 {
-   c_->getOdometry(x,y,w,vx,vy,vw);
+  c_->getOdometry(x,y,w,vx,vy,vw);
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-16 21:36:46 UTC (rev 5445)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-16 21:41:26 UTC (rev 5446)
@@ -153,23 +153,23 @@
      run_time = atof(argv[4]);
      run_time_set = true;
   }
-  node->advertise<std_msgs::BaseVel>("cmd_vel",10);
+  node->advertise<std_msgs::BaseVel>("cmd_vel",1);
   sleep(1);
   node->publish("cmd_vel",cmd);
   sleep(1);
 
   libTF::Vector ang_rates;
   ros::Time start_time = ros::Time::now();
-  ros::Duration sleep_time(0.1);
+  ros::Duration sleep_time(0.01);
   while(!done)
   {
      ros::Duration delta_time = ros::Time::now() - start_time;
-
+     cout << "Sending out command " << cmd.vx << " " << cmd.vy << " " << cmd.vw  << endl;
      if(run_time_set && delta_time.toSec() > run_time)
         break;
     //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
-    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
-    cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+     cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " << tb.ground_truth.transform.translation.x << " " << tb.ground_truth.transform.translation.y <<  " " << tb.ground_truth.transform.translation.z << " " <<  tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+    cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.pos.x <<  " " << tb.odom.pos.y << " " << tb.odom.pos.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
     //    cout << delta_time.toSec() << "  " << run_time << endl;
     node->publish("cmd_vel",cmd);
     sleep_time.sleep();


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5667
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5667&view=rev
Author:   mmwise
Date:     2008-10-22 20:53:07 +0000 (Wed, 22 Oct 2008)

Log Message:
-----------
changed track point to subscription

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-22 20:49:39 UTC (rev 5666)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-22 20:53:07 UTC (rev 5667)
@@ -48,7 +48,7 @@
 // Services
 #include <pr2_mechanism_controllers/SetJointCmd.h>
 #include <pr2_mechanism_controllers/GetJointCmd.h>
-#include <pr2_mechanism_controllers/TrackPoint.h>
+#include <std_msgs/PointStamped.h>
 
 // Math utils
 #include <math_utils/angles.h>
@@ -202,10 +202,9 @@
      * \param req  trackpoint (header, point)
      * \param resp (pan_angle, tilt_angle)
      */
-    bool trackPoint(pr2_mechanism_controllers::TrackPoint::request &req,                                    
-                     pr2_mechanism_controllers::TrackPoint::response &resp);
+    void trackPoint();
   private:
-
+    std_msgs::PointStamped track_point_; /**< The point from the subscription. */
     HeadPanTiltController *c_;      /**< The controller. */
     std::string service_prefix;     /**< The service name. */
     ros::node *node;                /**< The node. */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-10-22 20:49:39 UTC (rev 5666)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-10-22 20:53:07 UTC (rev 5667)
@@ -1,21 +1,50 @@
 #!/usr/bin/env python
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
 
+# Author: Melonee Wise
+
 PKG = "pr2_mechanism_controllers"
 
-import rostools; rostools.update_path(PKG) 
+import rostools; rostools.update_path(PKG)
+#rostools.update_path('std_msgs')
 
 import sys
 import os
 import string
+from time import sleep
 
 import rospy
-from std_msgs import *
+from std_msgs.msg import PointStamped, Point
+from pr2_mechanism_controllers.srv import SetJointCmd, SetJointCmdRequest
 
-from pr2_mechanism_controllers.srv import *
-
 def point_head_client(pan, tilt):
-
-    # NOTE: you don't have to call rospy.init_node() to make calls against
+    
+      # NOTE: you don't have to call rospy.init_node() to make calls against
     # a service. This is because service clients do not have to be
     # nodes.
 
@@ -36,30 +65,18 @@
     except rospy.ServiceException, e:
         print "Service call failed: %s"%e
 
+       
+
 def point_head_cart_client(x,y,z,frame):
 
-    # NOTE: you don't have to call rospy.init_node() to make calls against
-    # a service. This is because service clients do not have to be
-    # nodes.
-
-    # block until the add_two_ints service is available
-    print "waiting for service"
-    rospy.wait_for_service('head_controller/track_point')
     
-    try:
-        # create a handle to the add_two_ints service
-        send_cmd= rospy.ServiceProxy('head_controller/track_point', TrackPoint)
-        
-        print "Requesting %f,%f,%f"%(x, y, z)
-        
-        # simplified style
-        resp2 = send_cmd(std_msgs.msg.PointStamped(rostools.msg.Header(None, None, frame), std_msgs.msg.Point(x, y, z)))
-#        resp2 = send_cmd.call(TrackPointRequest( std_msgs.msg.PointStamped(None, std_msgs.msg.Point(x, y, z))))        #print resp2.positions, resp2.names
-        print resp2.pan_angle, resp2.tilt_angle
-        return resp2
-    except rospy.ServiceException, e:
-        print "Service call failed: %s"%e
+    head_angles = rospy.Publisher('head_controller/track_point', PointStamped)
+    rospy.init_node('head_commander', anonymous=True)
+    sleep(1)
+    head_angles.publish(PointStamped(rostools.msg.Header(None, None, frame), Point(x, y, z)))
 
+
+
 def usage():
     return "%s [pan tilt] or [x,y,z,frame]"%sys.argv[0]
 
@@ -72,5 +89,3 @@
         point_head_client(float(sys.argv[1]), float(sys.argv[2]))
     elif len(sys.argv) ==5:
         point_head_cart_client(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), sys.argv[4])
-
-

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-22 20:49:39 UTC (rev 5666)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-22 20:53:07 UTC (rev 5667)
@@ -172,6 +172,7 @@
 {
   node->unadvertise_service(service_prefix + "/set_command_array");
   node->unadvertise_service(service_prefix + "/get_command_array");
+  node->unsubscribe(service_prefix + "/track_point");
 
   delete c_;
 }
@@ -192,7 +193,8 @@
   {
     node->advertise_service(service_prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointCmd, this);
     node->advertise_service(service_prefix + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
-    node->advertise_service(service_prefix + "/track_point", &HeadPanTiltControllerNode::trackPoint, this);
+    node->subscribe(service_prefix + "/track_point", track_point_,
+                  &HeadPanTiltControllerNode::trackPoint, this, 2);
 
     return true;
   }
@@ -225,23 +227,25 @@
   return true;
 }
 
-bool HeadPanTiltControllerNode::trackPoint(pr2_mechanism_controllers::TrackPoint::request &req, pr2_mechanism_controllers::TrackPoint::response &resp)
+void HeadPanTiltControllerNode::trackPoint()
 {
   std::vector<double> pos;
   std::vector<std::string> names;
 
   libTF::TFPoint point;
-  point.x = req.target.point.x;
-  point.y = req.target.point.y;
-  point.z = req.target.point.z;
-  point.time = req.target.header.stamp.toNSec();
-  point.frame = req.target.header.frame_id;
+  point.x = track_point_.point.x;
+  point.y = track_point_.point.y;
+  point.z = track_point_.point.z;
+  point.time = track_point_.header.stamp.toNSec();
+  point.frame = track_point_.header.frame_id;
   
+
+  
   libTF::TFPoint pan_point = TF.transformPoint("head_pan",point);
   int id = c_->getJointControllerByName("head_pan_joint");
   assert(id>=0);
-  double mes_pan_angle = c_->joint_position_controllers_[id]->getMeasuredPosition();
-  double head_pan_angle= mes_pan_angle + atan2(pan_point.y, pan_point.x); 
+  double meas_pan_angle = c_->joint_position_controllers_[id]->getMeasuredPosition();
+  double head_pan_angle= meas_pan_angle + atan2(pan_point.y, pan_point.x); 
   
   names.push_back("head_pan_joint");
   pos.push_back(head_pan_angle);
@@ -249,17 +253,14 @@
   libTF::TFPoint tilt_point = TF.transformPoint("head_tilt",point);
   id = c_->getJointControllerByName("head_tilt_joint");
   assert(id>=0);
-  double mes_tilt_angle= c_->joint_position_controllers_[id]->getMeasuredPosition();
-  double head_tilt_angle= mes_tilt_angle + atan2(-tilt_point.z, tilt_point.x);
+  double meas_tilt_angle= c_->joint_position_controllers_[id]->getMeasuredPosition();
+  double head_tilt_angle= meas_tilt_angle + atan2(-tilt_point.z, tilt_point.x);
   
   names.push_back("head_tilt_joint");
   pos.push_back(head_tilt_angle);
+       
+  c_->setJointCmd(pos,names);
   
-     
-  c_->setJointCmd(pos,names);
-  resp.pan_angle = head_pan_angle;
-  resp.tilt_angle =head_tilt_angle;
-  return true;
 }
 
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5951
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5951&view=rev
Author:   mmwise
Date:     2008-10-29 22:26:10 +0000 (Wed, 29 Oct 2008)

Log Message:
-----------
changed transform to tf and changed the set joint from srv to sub

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-29 22:19:19 UTC (rev 5950)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-10-29 22:26:10 UTC (rev 5951)
@@ -46,13 +46,13 @@
 #include <robot_mechanism_controllers/joint_position_controller.h>
 
 // Services
-#include <robot_srvs/SetJointCmd.h>
+#include <robot_msgs/JointCmd.h>
 #include <robot_srvs/GetJointCmd.h>
 #include <std_msgs/PointStamped.h>
 
 // Math utils
 #include <math_utils/angles.h>
-#include <rosTF/rosTF.h>
+#include <tf/transform_listener.h>
 
 namespace controller
 {
@@ -182,11 +182,9 @@
     /*!
      * \brief Sets a command for all the joints managed by the controller at once.
      * 
-     * \param req (names, positions)
-     * \param resp (names, positions)
+     * \param joint_cmds_ (names, positions)
      */
-    bool setJointCmd(robot_srvs::SetJointCmd::request &req,
-                     robot_srvs::SetJointCmd::response &resp);
+    void setJointCmd();
 
     /*!
      * \brief Gets the commands for all the joints managed by the controller at once.
@@ -199,16 +197,16 @@
     /*!
      * \brief Tracks a point in a specified frame.
      * 
-     * \param req  trackpoint (header, point)
-     * \param resp (pan_angle, tilt_angle)
+     * \param track_point_ (header, point)
      */
     void trackPoint();
   private:
     std_msgs::PointStamped track_point_; /**< The point from the subscription. */
-    HeadPanTiltController *c_;      /**< The controller. */
-    std::string service_prefix;     /**< The service name. */
-    ros::node *node;                /**< The node. */
-    rosTFClient TF;                 /**< The transform for converting from point to head and tilt frames. */
+    robot_msgs::JointCmd joint_cmds_;    /**< The joint commands from the subscription.*/
+    HeadPanTiltController *c_;           /**< The controller. */
+    std::string service_prefix;          /**< The service name. */
+    ros::node *node;                     /**< The node. */
+    tf::TransformListener TF;            /**< The transform for converting from point to head and tilt frames. */
 
 };
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-10-29 22:19:19 UTC (rev 5950)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-10-29 22:26:10 UTC (rev 5951)
@@ -40,33 +40,15 @@
 
 import rospy
 from std_msgs.msg import PointStamped, Point
-from pr2_mechanism_controllers.srv import SetJointCmd, SetJointCmdRequest
+from robot_msgs.msg import JointCmd
 
 def point_head_client(pan, tilt):
-    
-      # NOTE: you don't have to call rospy.init_node() to make calls against
-    # a service. This is because service clients do not have to be
-    # nodes.
 
-    # block until the add_two_ints service is available
-    print "waiting for service"
-    rospy.wait_for_service('head_controller/set_command_array')
-    
-    try:
-        # create a handle to the add_two_ints service
-        send_cmd= rospy.ServiceProxy('head_controller/set_command_array', SetJointCmd)
-        
-        print "Requesting %f,%f"%(pan, tilt)
-        
-        # simplified style
-        resp2 = send_cmd.call(SetJointCmdRequest([pan, tilt],[0.0, 0.0],[0.0, 0.0],['head_pan_joint', 'head_tilt_joint']))
-        print resp2.positions, resp2.names
-        return resp2
-    except rospy.ServiceException, e:
-        print "Service call failed: %s"%e
+    head_angles = rospy.Publisher('head_controller/set_command_array', JointCmd)
+    rospy.init_node('head_commander', anonymous=True)
+    sleep(1)
+    head_angles.publish(JointCmd([pan, tilt],[0.0, 0.0],[0.0, 0.0],['head_pan_joint', 'head_tilt_joint']))
 
-       
-
 def point_head_cart_client(x,y,z,frame):
 
     

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-29 22:19:19 UTC (rev 5950)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-10-29 22:26:10 UTC (rev 5951)
@@ -39,6 +39,7 @@
 using namespace controller;
 using namespace std;
 
+
 ROS_REGISTER_CONTROLLER(HeadPanTiltController);
 
 HeadPanTiltController::HeadPanTiltController():num_joints_(0),last_time_(0)
@@ -170,7 +171,7 @@
 
 HeadPanTiltControllerNode::~HeadPanTiltControllerNode()
 {
-  node->unadvertise_service(service_prefix + "/set_command_array");
+  node->unsubscribe(service_prefix + "/set_command_array");
   node->unadvertise_service(service_prefix + "/get_command_array");
   node->unsubscribe(service_prefix + "/track_point");
 
@@ -191,7 +192,7 @@
   // Parses subcontroller configuration
   if(c_->initXml(robot, config))
   {
-    node->advertise_service(service_prefix + "/set_command_array", &HeadPanTiltControllerNode::setJointCmd, this);
+    node->subscribe(service_prefix + "/set_command_array", joint_cmds_, &HeadPanTiltControllerNode::setJointCmd, this,2);
     node->advertise_service(service_prefix + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
     node->subscribe(service_prefix + "/track_point", track_point_,
                   &HeadPanTiltControllerNode::trackPoint, this, 2);
@@ -201,21 +202,9 @@
   return false;
 }
 
-bool HeadPanTiltControllerNode::setJointCmd(robot_srvs::SetJointCmd::request &req,
-                                   robot_srvs::SetJointCmd::response &resp)
+void HeadPanTiltControllerNode::setJointCmd()
 {
-  std::vector<double> pos;
-  std::vector<std::string> names;
-  robot_msgs::JointCmd cmds;
-  req.get_positions_vec(pos);
-  req.get_names_vec(names);
-
-  c_->setJointCmd(pos,names);
-  c_->getJointCmd(cmds);
-  resp.set_positions_vec(pos);
-  resp.set_names_vec(cmds.names);
-  
-  return true;
+  c_->setJointCmd(joint_cmds_.positions,joint_cmds_.names); 
 }
 
 bool HeadPanTiltControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
@@ -232,29 +221,31 @@
   std::vector<double> pos;
   std::vector<std::string> names;
 
-  libTF::TFPoint point;
-  point.x = track_point_.point.x;
-  point.y = track_point_.point.y;
-  point.z = track_point_.point.z;
-  point.time = track_point_.header.stamp.toNSec();
-  point.frame = track_point_.header.frame_id;
+  tf::Stamped<tf::Point> point;
+  point.setX(track_point_.point.x);
+  point.setY(track_point_.point.y);
+  point.setZ(track_point_.point.z);
+  point.stamp_ = track_point_.header.stamp;
+  point.frame_id_ = track_point_.header.frame_id;
   
 
   
-  libTF::TFPoint pan_point = TF.transformPoint("head_pan",point);
+  tf::Stamped<tf::Point> pan_point;
+  TF.transformPoint("head_pan",point, pan_point);
   int id = c_->getJointControllerByName("head_pan_joint");
   assert(id>=0);
   double meas_pan_angle = c_->joint_position_controllers_[id]->getMeasuredPosition();
-  double head_pan_angle= meas_pan_angle + atan2(pan_point.y, pan_point.x); 
+  double head_pan_angle= meas_pan_angle + atan2(pan_point.y(), pan_point.x()); 
   
   names.push_back("head_pan_joint");
   pos.push_back(head_pan_angle);
   
-  libTF::TFPoint tilt_point = TF.transformPoint("head_tilt",point);
+  tf::Stamped<tf::Point> tilt_point;
+  TF.transformPoint("head_tilt",point,tilt_point);
   id = c_->getJointControllerByName("head_tilt_joint");
   assert(id>=0);
   double meas_tilt_angle= c_->joint_position_controllers_[id]->getMeasuredPosition();
-  double head_tilt_angle= meas_tilt_angle + atan2(-tilt_point.z, tilt_point.x);
+  double head_tilt_angle= meas_tilt_angle + atan2(-tilt_point.z(), tilt_point.x());
   
   names.push_back("head_tilt_joint");
   pos.push_back(head_tilt_angle);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 5982
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5982&view=rev
Author:   sachinchitta
Date:     2008-10-30 01:17:12 +0000 (Thu, 30 Oct 2008)

Log Message:
-----------
Changed test_odom to print the correct info out. 
Changed base controller to interpolate between commands using a linear interpolation.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 01:15:50 UTC (rev 5981)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 01:17:12 UTC (rev 5982)
@@ -38,7 +38,7 @@
 #include "ros/node.h"
 
 #define NUM_TRANSFORMS 2
-
+#define EPS 1e-5
 using namespace ros;
 using namespace std;
 using namespace controller;
@@ -118,6 +118,7 @@
   pthread_mutex_unlock(&base_controller_lock_);
 }
 
+/*
 libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
 {
   libTF::Vector result;
@@ -126,7 +127,47 @@
   result.z = start.z + clamp(end.z - start.z,-max_rate.z*dT,max_rate.z*dT);
   return result;
 }
+*/
 
+libTF::Vector BaseController::interpolateCommand(libTF::Vector start, libTF::Vector end, libTF::Vector max_rate, double dT)
+{
+  libTF::Vector result;
+  libTF::Vector alpha;
+  double delta(0), max_delta(0);
+
+  delta = end.x - start.x;
+  max_delta = max_rate.x * dT;
+  if(fabs(delta) <= max_delta || max_delta < EPS)
+    alpha.x = 1;
+  else
+    alpha.x = max_delta/fabs(delta);
+
+  delta = end.y - start.y;
+  max_delta = max_rate.y * dT;
+  if(fabs(delta) <= max_delta || max_delta < EPS)
+    alpha.y = 1;
+  else
+    alpha.y = max_delta/fabs(delta);
+
+  delta = end.z - start.z;
+  max_delta = max_rate.z * dT;
+  if(fabs(delta) <= max_delta || max_delta < EPS)
+    alpha.z = 1;
+  else
+    alpha.z = max_delta/fabs(delta);
+
+  double alpha_min = alpha.x;
+  if(alpha.y < alpha_min)
+    alpha_min = alpha.y;
+  if(alpha.z < alpha_min)
+    alpha_min = alpha.z;
+
+  result.x = start.x + alpha_min*(end.x - start.x);
+  result.y = start.y + alpha_min*(end.y - start.y);
+  result.z = start.z + alpha_min*(end.z - start.z);
+  return result;
+}
+
 libTF::Vector BaseController::getCommand()// Return the current position command
 {
   libTF::Vector cmd_vel;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2008-10-30 01:15:50 UTC (rev 5981)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2008-10-30 01:17:12 UTC (rev 5982)
@@ -34,7 +34,6 @@
 
 #include <libTF/libTF.h>
 #include <ros/node.h>
-#include <std_msgs/TransformWithRateStamped.h>
 #include <std_msgs/BaseVel.h>
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/Quaternion.h>
@@ -84,30 +83,16 @@
 {
   public:
 
-    test_run_base():subscriber_connected(0){}; 
+    test_run_base(){}; 
 
     ~test_run_base() {}
 
-    std_msgs::TransformWithRateStamped ground_truth;
-
     std_msgs::RobotBase2DOdom odom;
 
-    int subscriber_connected;
-
-    void subscriber_connect()
-    {
-      subscriber_connected = 1;
-    }
-
     void odomMsgReceived()
     {
-//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
     };
 
-    void groundTruthMsgReceived()
-    {
-//       cout << "Odom:: (" << ground_truth.transform.translation.x << "), (" <<  ground_truth.transform.rotation.x << ")" << std::endl;  
-    };
 };
 
 int main( int argc, char** argv )
@@ -117,13 +102,9 @@
   ros::init(argc,argv);
   ros::node *node = new ros::node("test_run_base_controller"); 
 
-
-  // receive messages from 2dnav stack
-  std_msgs::TransformWithRateStamped ground_truth;
-
   test_run_base tb;
 
-  node->subscribe("odom",tb.odom,&test_run_base::groundTruthMsgReceived,&tb,10);
+  node->subscribe("odom",tb.odom,&test_run_base::odomMsgReceived,&tb,10);
 
   signal(SIGINT,  finalize);
   signal(SIGQUIT, finalize);
@@ -179,16 +160,10 @@
 
      if(run_time_set && delta_time.toSec() > run_time)
         break;
-    //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
 
-//     odom_log_file << 
-
-//    cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " <<   tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
-
      odom_log_file << tb.odom.pos.x <<  " " << tb.odom.pos.y << " " << tb.odom.pos.th << " " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
 
-     //   cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
-     cout << delta_time.toSec() << "  " << run_time << endl;
+     cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.vel.x <<  endl << " y:" << tb.odom.vel.y << endl << " omega:" << tb.odom.vel.th << std::endl;
     node->publish("cmd_vel",cmd);
     sleep_time.sleep();
   }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-30 01:15:50 UTC (rev 5981)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2008-10-30 01:17:12 UTC (rev 5982)
@@ -158,7 +158,7 @@
   node->publish("cmd_vel",cmd);
   sleep(1);
 
-  libTF::Vector ang_rates;
+  libTF::Vector ground_truth_angles;
   ros::Time start_time = ros::Time::now();
   ros::Duration sleep_time(0.01);
   while(!done)
@@ -168,7 +168,8 @@
      if(run_time_set && delta_time.toSec() > run_time)
         break;
     //   ang_rates = GetAsEuler(tb.ground_truth.rate.rotation);
-     cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " << tb.ground_truth.transform.translation.x << " " << tb.ground_truth.transform.translation.y <<  " " << tb.ground_truth.transform.translation.z << " " <<  tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+     ground_truth_angles = GetAsEuler(tb.ground_truth.transform.rotation);
+     cout << "g:: " << tb.ground_truth.rate.translation.x <<  " " << tb.ground_truth.rate.translation.y << " "  << tb.ground_truth.rate.rotation.z  << " " << tb.ground_truth.transform.translation.x << " " << tb.ground_truth.transform.translation.y <<  " " << ground_truth_angles.z << " " <<  tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
     cout << "o:: " << tb.odom.vel.x <<  " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.pos.x <<  " " << tb.odom.pos.y << " " << tb.odom.pos.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
     //    cout << delta_time.toSec() << "  " << run_time << endl;
     node->publish("cmd_vel",cmd);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6022
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6022&view=rev
Author:   sachinchitta
Date:     2008-10-30 19:10:15 +0000 (Thu, 30 Oct 2008)

Log Message:
-----------
Added publishing of base_controller/odometer message with two files 
distance: distance travelled while base controller was up
angle: angle rotated through while base controller was up

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-30 19:09:11 UTC (rev 6021)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-10-30 19:10:15 UTC (rev 6022)
@@ -54,6 +54,7 @@
 
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/BaseVel.h>
+#include <pr2_msgs/Odometer.h>
 
 #include <misc_utils/realtime_publisher.h>
 
@@ -377,6 +378,11 @@
 
     std::map<std::string, double*> param_map_; /*< map from pointers to the params to string names */
 
+    double odometer_distance_;
+
+    double odometer_angle_;
+
+    friend class BaseControllerNode;
   };
 
   /*! \class
@@ -459,6 +465,8 @@
 
     misc_utils::RealtimePublisher <rosTF::TransformArray>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
+    misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+
   };
 
     /** \brief A namespace ostream overload for displaying parameters */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-30 19:09:11 UTC (rev 6021)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-30 19:10:15 UTC (rev 6022)
@@ -14,6 +14,7 @@
   <depend package="libTF" />
   <depend package="rosTF" />
   <depend package="std_msgs" />
+  <depend package="pr2_msgs" />
   <depend package="math_utils" />
   <depend package="robot_kinematics" />
   <url>http://pr.willowgarage.com</url>

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 19:09:11 UTC (rev 6021)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 19:10:15 UTC (rev 6022)
@@ -86,6 +86,9 @@
    caster_steer_vel_gain_ = 0;
    timeout_ = 0.1;
 
+   odometer_distance_ = 0;
+   odometer_angle_ = 0;
+
    new_cmd_available_ = false;
 
    pthread_mutex_init(&base_controller_lock_,NULL);
@@ -561,6 +564,9 @@
 
   base_odom_delta.z = base_odom_velocity_.z * dt;
   base_odom_position_ += base_odom_delta;
+
+  odometer_distance_ += sqrt(base_odom_delta.x*base_odom_delta.x + base_odom_delta.y*base_odom_delta.y);
+  odometer_angle_ += base_odom_delta.z;
 }
 
 void BaseController::computeBaseVelocity()
@@ -725,6 +731,7 @@
   odom_publish_delta_t_ = 1.0/odom_publish_rate_;
   publisher_ = NULL;
   transform_publisher_ = NULL;
+  odometer_publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
@@ -737,6 +744,7 @@
   transform_publisher_->stop();
   delete publisher_;
   delete transform_publisher_;
+  delete odometer_publisher_;
   delete c_;
 }
 
@@ -749,6 +757,14 @@
 
   if (time-last_time_message_sent_ >= odom_publish_delta_t_) // send odom message
   {
+
+    if (odometer_publisher_->trylock())
+    {
+      odometer_publisher_->msg_.distance = c_->odometer_distance_; 
+      odometer_publisher_->msg_.angle = c_->odometer_angle_; 
+      odometer_publisher_->unlockAndPublish();      
+    }
+
     if (publisher_->trylock())
     {
       c_->setOdomMessage(publisher_->msg_);
@@ -846,6 +862,9 @@
     delete publisher_ ;
   publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
 
+  if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete odometer_publisher_ ;
+  odometer_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
 
   if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete transform_publisher_ ;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6046
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6046&view=rev
Author:   sachinchitta
Date:     2008-10-30 22:55:32 +0000 (Thu, 30 Oct 2008)

Log Message:
-----------
Switched i/o to rosout in base_controller.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-30 22:48:34 UTC (rev 6045)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-10-30 22:55:32 UTC (rev 6046)
@@ -17,6 +17,7 @@
   <depend package="pr2_msgs" />
   <depend package="math_utils" />
   <depend package="robot_kinematics" />
+  <depend package="rosout" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 22:48:34 UTC (rev 6045)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 22:55:32 UTC (rev 6046)
@@ -108,13 +108,16 @@
   cmd_vel_t_.z = clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 #if 0
-  std::cout << "BaseController:: command received: " << cmd_vel;
-  std::cout << "BaseController:: command current: " << cmd_vel_;
-  std::cout << "Base Odometry: Velocity " << base_odom_velocity_;
-  std::cout << "Base Odometry: Position " << base_odom_position_;
+
+  ROS_INFO("BaseController:: command received: %f %f %f",cmd_vel.x,cmd_vel.y,cmd_vel.z);
+  ROS_INFO("BaseController:: command current: %f %f %f", cmd_vel_.x,cmd_vel_.y,cmd_vel_.z);
+
+  ROS_INFO("Base Odometry: Velocity ", base_odom_velocity_.x,base_odom_velocity_.y,base_odom_velocity_.z);
+  ROS_INFO("Base Odometry: Position ", base_odom_position_.x,base_odom_position_.y,base_odom_position_.z);
+
   for(int i=0; i < (int) num_wheels_; i++)
   {
-    std:: cout << "wheel speed cmd:: " << i << "  " << (base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]) << endl;
+    ROS_INFO("BaseController:: wheel speed cmd:: %d %f",i,(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]));
   }
 #endif
   new_cmd_available_ = true;
@@ -194,7 +197,7 @@
   // wait for robotdesc/pr2 on param server
   while(!urdf_model_.loadString(xml_content.c_str()))
   {
-    std::cout << "WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar." << std::endl;
+    ROS_INFO("WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar.");
     (ros::g_node)->get_param("robotdesc/pr2",xml_content);
     usleep(100000);
   }
@@ -212,7 +215,9 @@
     base_object.parent_ = NULL;
     base_object.joint_state_ = robot_state->getJointState(joint_name);
     if (base_object.joint_state_==NULL)
-      std::cout << " unsuccessful getting joint state for " << joint_name << std::endl;
+    {
+      ROS_WARN(" Unsuccessful getting joint state for %s",joint_name.c_str());
+    }
 
     base_object.controller_.init(robot_state, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
 
@@ -224,7 +229,7 @@
       steer_angle_actual_.push_back(0);
       steer_velocity_desired_.push_back(0);
       num_casters_++;
-      cout << "base_casters" << "::  " << base_object;
+//      cout << "base_casters" << "::  " << base_object;
     }
 
     if(joint_name.find("wheel") != string::npos)
@@ -281,7 +286,7 @@
 bool BaseController::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
 
-  std::cout << " base controller name: " << config->Attribute("name") << std::endl;
+  ROS_INFO("BaseController:: name: %s",config->Attribute("name")); 
   TiXmlElement *elt = config->FirstChildElement("controller");
   std::vector<JointControlParam> jcp_vec;
   JointControlParam jcp;
@@ -295,13 +300,13 @@
     jcp.joint_name = jnt->Attribute("name");
     jcp_vec.push_back(jcp);
 
-    std::cout << "name:" << jcp.joint_name << std::endl;
-    std::cout << "controller type:" << jcp.control_type << std::endl;
-    std::cout << "sub controller: " << elt->Attribute("name") << std::endl;
+    ROS_INFO("BaseController:: joint name: %s",jcp.joint_name.c_str()); 
+    ROS_INFO("BaseController:: controller type: %s\n",jcp.control_type.c_str()); 
 
     elt = elt->NextSiblingElement("controller");
   }
 
+
   addParamToMap("kp_speed",&kp_speed_);
   addParamToMap("kp_caster_steer",&caster_steer_vel_gain_);
   addParamToMap("timeout",&timeout_);
@@ -325,21 +330,24 @@
         elt_key = elt_key->NextSiblingElement("elem");
       }
     }
-    std::cout << "*************************************" << std::endl;
+//    std::cout << "*************************************" << std::endl;
     elt = config->NextSiblingElement("map");
   }
 
-  cout << "kp_speed  " << kp_speed_ << endl;
-  cout << "kp_caster_steer  " << caster_steer_vel_gain_ << endl;
-  cout << "timeout  " << timeout_ << endl;
-  cout << "max_x_dot  " << (max_vel_.x) << endl;
-  cout << "max_y_dot  " << (max_vel_.y) << endl;
-  cout << "max_yaw_dot  " << (max_vel_.z) << endl;
-  cout << "max_x_accel  " << (max_accel_.x) << endl;
-  cout << "max_y_accel  " << (max_accel_.y) << endl;
-  cout << "max_yaw_accel  " << (max_accel_.z) << endl;
 
+  ROS_INFO("BaseController:: kp_speed %f",kp_speed_);
+  ROS_INFO("BaseController:: kp_caster_steer  %f",caster_steer_vel_gain_);
+  ROS_INFO("BaseController:: timeout %f",timeout_);
+  ROS_INFO("BaseController:: max_x_dot %f",(max_vel_.x));
+  ROS_INFO("BaseController:: max_y_dot %f",(max_vel_.y));
+  ROS_INFO("BaseController:: max_yaw_dot %f",(max_vel_.z));
+  ROS_INFO("BaseController:: max_x_accel %f",(max_accel_.x));
+  ROS_INFO("BaseController:: max_y_accel %f",(max_accel_.y));
+  ROS_INFO("BaseController:: max_yaw_accel %f",(max_accel_.z));
+
   init(jcp_vec,robot_state);
+
+  ROS_INFO("BaseController:: Initialized");
   return true;
 }
 
@@ -363,7 +371,7 @@
     speeds_are_valid = speeds_are_valid && !isinf(wheel_speed_actual_[i]) && !isnan(wheel_speed_actual_[i]);
 
   if(!speeds_are_valid)
-    fprintf(stderr,"BaseController:: input speed values are inf or nan\n");
+    ROS_WARN("BaseController:: input speed values are inf or nan");
 }
 
 void BaseController::computeWheelPositions()
@@ -688,7 +696,6 @@
     if(weight_type == std::string("L1norm"))
     {
       w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
-      cout << residual(i,1) << endl;
     }
     else if(weight_type == std::string("fair"))
     {
@@ -701,7 +708,6 @@
     else if(weight_type == std::string("BubeLangan"))
     {
       w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
-      cout << residual(i,1) << endl;
     }
     else if(weight_type == std::string("Gaussian"))
     {
@@ -709,7 +715,6 @@
     }
     else // default to fair
     {
-      cout << "Default" << endl;
       w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
     }
   }
@@ -854,7 +859,7 @@
     return false;
 
   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
-  node->advertise_service(service_prefix + "/get_actual", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+  node->advertise_service(service_prefix + "/get_command", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
 
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6050
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6050&view=rev
Author:   sachinchitta
Date:     2008-10-30 23:23:19 +0000 (Thu, 30 Oct 2008)

Log Message:
-----------
Modified the service fields to match the names of topic fields. 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 23:17:49 UTC (rev 6049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-10-30 23:23:19 UTC (rev 6050)
@@ -814,14 +814,14 @@
   pr2_mechanism_controllers::SetBaseCommand::response &resp)
 {
   libTF::Vector command;
-  command.x = req.x_vel;
-  command.y = req.y_vel;
-  command.z = req.theta_vel;
+  command.x = req.vx;
+  command.y = req.vy;
+  command.z = req.vw;
   c_->setCommand(command);
   command = c_->getCommand();
-  resp.x_vel = command.x;
-  resp.y_vel = command.y;
-  resp.theta_vel = command.z;
+  resp.vx = command.x;
+  resp.vy = command.y;
+  resp.vw = command.z;
   return true;
 }
 
@@ -842,9 +842,9 @@
 {
   libTF::Vector command;
   command = c_->getCommand();
-  resp.x_vel = command.x;
-  resp.y_vel = command.y;
-  resp.theta_vel = command.z;
+  resp.vx = command.x;
+  resp.vy = command.y;
+  resp.vw = command.z;
 
   return true;
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv	2008-10-30 23:17:49 UTC (rev 6049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GetBaseCommand.srv	2008-10-30 23:23:19 UTC (rev 6050)
@@ -1,4 +1,4 @@
 ---
-float64 x_vel
-float64 y_vel
-float64 theta_vel
+float64 vx
+float64 vy
+float64 vw

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv	2008-10-30 23:17:49 UTC (rev 6049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetBaseCommand.srv	2008-10-30 23:23:19 UTC (rev 6050)
@@ -1,7 +1,7 @@
-float64 x_vel
-float64 y_vel
-float64 theta_vel
+float64 vx
+float64 vy
+float64 vw
 ---
-float64 x_vel
-float64 y_vel
-float64 theta_vel
+float64 vx
+float64 vy
+float64 vw


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6344
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6344&view=rev
Author:   mmwise
Date:     2008-11-06 23:28:53 +0000 (Thu, 06 Nov 2008)

Log Message:
-----------
added service to set the angles so that a specific frame is tracking a point

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-06 23:25:35 UTC (rev 6343)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-06 23:28:53 UTC (rev 6344)
@@ -52,6 +52,7 @@
 
 // Math utils
 #include <math_utils/angles.h>
+#include <math.h>
 #include <tf/transform_listener.h>
 
 namespace controller
@@ -69,19 +70,19 @@
 
     <controller name="head_controller" type="HeadPanTiltControllerNode"><br>
       <listen_topic name="head_commands" /><br>
-    
+
       <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionController"><br>
         <joint name="head_pan_joint" ><br>
           <pid p="1.5" d="0.1" i="0.3" iClamp="0.2" /><br>
         </joint><br>
       </controller><br>
-      
+
       <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionController"><br>
         <joint name="head_tilt_joint" ><br>
           <pid p="0.8" d="0.05" i="0.1" iClamp="0.1" /><br>
         </joint><br>
       </controller><br>
-    </controller> <br> 
+    </controller> <br>
 */
 /***************************************************/
 
@@ -132,10 +133,10 @@
    * \brief Issues commands to the joint.
    */
   virtual void update(void); // Real time safe.
-  
+
   unsigned int num_joints_; /**< Number of joints. */
   controller::JointPositionController* getJointPositionControllerByName(std::string name); /**< Joints we're controlling. */
-  std::vector<JointPositionController *> joint_position_controllers_; /**< Vector of the joint controllers. */ 
+  std::vector<JointPositionController *> joint_position_controllers_; /**< Vector of the joint controllers. */
 
 
 private:
@@ -147,7 +148,7 @@
   /*!
    * \brief Issues commands to the joint.
    */
-  void updateJointControllers(void); 
+  void updateJointControllers(void);
 
 };
 
@@ -164,7 +165,7 @@
 class HeadPanTiltControllerNode : public Controller
 {
   public:
-    
+
     HeadPanTiltControllerNode();
     ~HeadPanTiltControllerNode();
 
@@ -173,37 +174,46 @@
 
     /*!
      * \brief Sets a command for all the joints managed by the controller at once.
-     * 
+     *
      * \param joint_cmds_ (names, positions)
      */
     void setJointCmd();
 
     /*!
      * \brief Gets the commands for all the joints managed by the controller at once.
-     * 
-     * \param req 
+     *
+     * \param req
      * \param resp (positions)
      */
     bool getJointCmd(robot_srvs::GetJointCmd::request &req,
                      robot_srvs::GetJointCmd::response &resp);
     /*!
-     * \brief Tracks a point in a specified frame.
-     * 
-     * \param track_point_ (header, point)
+     * \brief This points the head at a specified point from a given frame.
+     *
+     * \param head_track_point_ (header, point)
      */
-    void trackPoint();
+    void headTrackPoint();
+    /*!
+     * \brief This points the given frame at a specified point from the given frame.
+     *
+     * \param frame_track_point_ (header, point)
+     */
+    void frameTrackPoint();
 
   private:
 
     //node stuff
-    std::string service_prefix_;                  /**< The service name. */
-    ros::node *node_;                             /**< The node. */
-    AdvertisedServiceGuard guard_get_command_array_;   /**< Makes sure the advertise goes down neatly. */
-    SubscriptionGuard guard_set_command_array_ , guard_track_point_;        /**< Makes sure the subscription goes down neatly. */
+    std::string service_prefix_;                         /**< The service name. */
+    ros::node *node_;                                    /**< The node. */
+    AdvertisedServiceGuard guard_get_command_array_;     /**< Makes sure the advertise goes down neatly. */
+    SubscriptionGuard guard_set_command_array_;          /**< Makes sure the subscription goes down neatly. */
+    SubscriptionGuard guard_head_track_point_;           /**< Makes sure the subscription goes down neatly. */
+    SubscriptionGuard guard_frame_track_point_;           /**< Makes sure the subscription goes down neatly. */
 
-    //msgs 
-    std_msgs::PointStamped track_point_;         /**< The point from the subscription. */
-    robot_msgs::JointCmd joint_cmds_;            /**< The joint commands from the subscription.*/
+    //msgs
+    std_msgs::PointStamped head_track_point_;          /**< The point from the subscription. */
+    std_msgs::PointStamped frame_track_point_;         /**< The point from the subscription. */
+    robot_msgs::JointCmd joint_cmds_;                  /**< The joint commands from the subscription.*/
 
     //controller
     HeadPanTiltController *c_;                   /**< The controller. */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-11-06 23:25:35 UTC (rev 6343)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-11-06 23:28:53 UTC (rev 6344)
@@ -147,11 +147,11 @@
 {
 
   for(unsigned int i=0;i<num_joints_;++i)
-  { 
+  {
     if (!joint_position_controllers_[i]->joint_state_->calibrated_)
       return;  // joints are not calibrated
-      
-    joint_position_controllers_[i]->update(); 
+
+    joint_position_controllers_[i]->update();
   }
 }
 
@@ -162,7 +162,7 @@
 HeadPanTiltControllerNode::HeadPanTiltControllerNode()
 : Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL, 1000000000ULL)
 {
-  c_ = new HeadPanTiltController();  
+  c_ = new HeadPanTiltController();
 }
 
 HeadPanTiltControllerNode::~HeadPanTiltControllerNode()
@@ -179,26 +179,28 @@
 {
   assert(node_);
   service_prefix_ = config->Attribute("name");
-  
+
   // Parses subcontroller configuration
   if(!c_->initXml(robot, config))
     return false;
   //suscriptions
   node_->subscribe(service_prefix_ + "/set_command_array", joint_cmds_, &HeadPanTiltControllerNode::setJointCmd, this,1);
   guard_set_command_array_.set(service_prefix_ + "/set_command_array");
-  node_->subscribe(service_prefix_ + "/track_point", track_point_, &HeadPanTiltControllerNode::trackPoint, this, 1);
-  guard_track_point_.set(service_prefix_ + "/track_point");
+  node_->subscribe(service_prefix_ + "/head_track_point", head_track_point_, &HeadPanTiltControllerNode::headTrackPoint, this, 1);
+  guard_head_track_point_.set(service_prefix_ + "/head_track_point");
+  node_->subscribe(service_prefix_ + "/frame_track_point", frame_track_point_, &HeadPanTiltControllerNode::frameTrackPoint, this, 1);
+  guard_frame_track_point_.set(service_prefix_ + "/head_track_point");
   //services
   node_->advertise_service(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
   guard_get_command_array_.set(service_prefix_ + "/get_command_array");
 
   return true;
- 
+
 }
 
 void HeadPanTiltControllerNode::setJointCmd()
 {
-  c_->setJointCmd(joint_cmds_.positions,joint_cmds_.names); 
+  c_->setJointCmd(joint_cmds_.positions,joint_cmds_.names);
 }
 
 bool HeadPanTiltControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
@@ -210,20 +212,18 @@
   return true;
 }
 
-void HeadPanTiltControllerNode::trackPoint()
+void HeadPanTiltControllerNode::headTrackPoint()
 {
   std::vector<double> pos;
   std::vector<std::string> names;
 
   tf::Stamped<tf::Point> point;
-  point.setX(track_point_.point.x);
-  point.setY(track_point_.point.y);
-  point.setZ(track_point_.point.z);
-  point.stamp_ = track_point_.header.stamp;
-  point.frame_id_ = track_point_.header.frame_id;
-  
+  point.setX(head_track_point_.point.x);
+  point.setY(head_track_point_.point.y);
+  point.setZ(head_track_point_.point.z);
+  point.stamp_ = head_track_point_.header.stamp;
+  point.frame_id_ = head_track_point_.header.frame_id;
 
-  
   tf::Stamped<tf::Point> pan_point;
 
   try{
@@ -236,12 +236,13 @@
   int id = c_->getJointControllerByName("head_pan_joint");
   assert(id>=0);
   double meas_pan_angle = c_->joint_position_controllers_[id]->joint_state_->position_;
-  double head_pan_angle= meas_pan_angle + atan2(pan_point.y(), pan_point.x()); 
-  
+  double head_pan_angle= meas_pan_angle + atan2(pan_point.y(), pan_point.x());
+
   names.push_back("head_pan_joint");
   pos.push_back(head_pan_angle);
-  
+
   tf::Stamped<tf::Point> tilt_point;
+
   try{
     TF.transformPoint("head_tilt",point,tilt_point);
   }
@@ -254,13 +255,81 @@
   assert(id>=0);
   double meas_tilt_angle= c_->joint_position_controllers_[id]->joint_state_->position_;
   double head_tilt_angle= meas_tilt_angle + atan2(-tilt_point.z(), tilt_point.x());
-  
+
   names.push_back("head_tilt_joint");
   pos.push_back(head_tilt_angle);
-       
+
   c_->setJointCmd(pos,names);
-  
+
 }
 
 
+void HeadPanTiltControllerNode::frameTrackPoint()
+{
+  std::vector<double> pos;
+  std::vector<std::string> names;
+  tf::Stamped<tf::Transform> frame;
 
+  tf::Stamped<tf::Point> point;
+  point.setX(frame_track_point_.point.x);
+  point.setY(frame_track_point_.point.y);
+  point.setZ(frame_track_point_.point.z);
+  point.stamp_ = frame_track_point_.header.stamp;
+  point.frame_id_ = frame_track_point_.header.frame_id;
+
+  try
+  {
+    TF.lookupTransform(point.frame_id_,"head_pan",0,frame);
+  }
+  catch(tf::TransformException& ex)
+  {
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+  tf::Stamped<tf::Point> pan_point;
+
+  try
+  {
+    TF.transformPoint("head_pan",point, pan_point);
+  }
+  catch(tf::TransformException& ex)
+  {
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+  int id = c_->getJointControllerByName("head_pan_joint");
+  assert(id>=0);
+  double radius = pow(pan_point.x(),2)+pow(pan_point.y(),2);
+  double x_intercept = sqrt(radius-pow(frame.getOrigin().y(),2));
+  double delta_theta = atan2(pan_point.y(), pan_point.x())-atan2(frame.getOrigin().y(),x_intercept);
+  double meas_pan_angle = c_->joint_position_controllers_[id]->joint_state_->position_;
+  double head_pan_angle= meas_pan_angle + delta_theta;
+
+  names.push_back("head_pan_joint");
+  pos.push_back(head_pan_angle);
+
+  tf::Stamped<tf::Point> tilt_point;
+  try{
+    TF.transformPoint("head_tilt",point,tilt_point);
+  }
+  catch(tf::TransformException& ex){
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+  id = c_->getJointControllerByName("head_tilt_joint");
+  assert(id>=0);
+  radius = pow(tilt_point.x(),2)+pow(tilt_point.y(),2);
+  x_intercept = sqrt(radius-pow(frame.getOrigin().z(),2));
+  delta_theta = atan2(-tilt_point.z(), tilt_point.x())-atan2(-frame.getOrigin().z(),x_intercept);
+  double meas_tilt_angle= c_->joint_position_controllers_[id]->joint_state_->position_;
+  double head_tilt_angle= meas_tilt_angle + delta_theta;
+
+  names.push_back("head_tilt_joint");
+  pos.push_back(head_tilt_angle);
+
+  c_->setJointCmd(pos,names);
+
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6402
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6402&view=rev
Author:   mmwise
Date:     2008-11-07 21:16:56 +0000 (Fri, 07 Nov 2008)

Log Message:
-----------
added visualization markers

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-07 21:03:45 UTC (rev 6401)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_pan_tilt_controller.h	2008-11-07 21:16:56 UTC (rev 6402)
@@ -49,6 +49,7 @@
 #include <robot_msgs/JointCmd.h>
 #include <robot_srvs/GetJointCmd.h>
 #include <std_msgs/PointStamped.h>
+#include <std_msgs/VisualizationMarker.h>
 
 // Math utils
 #include <math_utils/angles.h>

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-11-07 21:03:45 UTC (rev 6401)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/pointhead.py	2008-11-07 21:16:56 UTC (rev 6402)
@@ -48,17 +48,17 @@
     rospy.init_node('head_commander', anonymous=True)
     sleep(1)
     head_angles.publish(JointCmd(['head_pan_joint', 'head_tilt_joint'],[0.0,0.0],[pan, tilt],[0.0, 0.0],[0.0, 0.0]))
+    sleep(1)
 
 def point_head_cart_client(x,y,z,frame):
 
-
-    head_angles = rospy.Publisher('head_controller/head_track_point', PointStamped)
+    head_angles = rospy.Publisher('head_controller/frame_track_point', PointStamped)
     rospy.init_node('head_commander', anonymous=True)
     sleep(1)
     head_angles.publish(PointStamped(rostools.msg.Header(None, None, frame), Point(x, y, z)))
+    sleep(1)
 
 
-
 def usage():
     return "%s [pan tilt] or [x,y,z,frame]"%sys.argv[0]
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-11-07 21:03:45 UTC (rev 6401)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_pan_tilt_controller.cpp	2008-11-07 21:16:56 UTC (rev 6402)
@@ -189,11 +189,11 @@
   node_->subscribe(service_prefix_ + "/head_track_point", head_track_point_, &HeadPanTiltControllerNode::headTrackPoint, this, 1);
   guard_head_track_point_.set(service_prefix_ + "/head_track_point");
   node_->subscribe(service_prefix_ + "/frame_track_point", frame_track_point_, &HeadPanTiltControllerNode::frameTrackPoint, this, 1);
-  guard_frame_track_point_.set(service_prefix_ + "/head_track_point");
+  guard_frame_track_point_.set(service_prefix_ + "/frame_track_point");
   //services
   node_->advertise_service(service_prefix_ + "/get_command_array", &HeadPanTiltControllerNode::getJointCmd, this);
   guard_get_command_array_.set(service_prefix_ + "/get_command_array");
-
+  node_->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
   return true;
 
 }
@@ -298,7 +298,6 @@
     ROS_WARN("Transform Exception %s", ex.what());
     return;
   }
-
   int id = c_->getJointControllerByName("head_pan_joint");
   assert(id>=0);
   double radius = pow(pan_point.x(),2)+pow(pan_point.y(),2);
@@ -319,11 +318,12 @@
     return;
   }
 
+
   id = c_->getJointControllerByName("head_tilt_joint");
   assert(id>=0);
   radius = pow(tilt_point.x(),2)+pow(tilt_point.y(),2);
   x_intercept = sqrt(radius-pow(frame.getOrigin().z(),2));
-  delta_theta = atan2(-tilt_point.z(), tilt_point.x())-atan2(-frame.getOrigin().z(),x_intercept);
+  delta_theta = atan2(-tilt_point.z(), tilt_point.x())-atan2(frame.getOrigin().z(),x_intercept);
   double meas_tilt_angle= c_->joint_position_controllers_[id]->joint_state_->position_;
   double head_tilt_angle= meas_tilt_angle + delta_theta;
 
@@ -332,4 +332,44 @@
 
   c_->setJointCmd(pos,names);
 
+  std_msgs::VisualizationMarker marker;
+  marker.header.frame_id = "stereo";
+  marker.id = 0;
+  marker.type = 0; 
+  marker.action = 0;
+  marker.x = 0;
+  marker.y = 0;
+  marker.z = 0;
+  marker.yaw = 0;
+  marker.pitch = 0;
+  marker.roll = 0.0;
+  marker.xScale = 2;
+  marker.yScale = 0.05;
+  marker.zScale = 0.05;
+  marker.alpha = 255;
+  marker.r = 0;
+  marker.g = 255;
+  marker.b = 0;
+  node_->publish("visualizationMarker", marker );
+
+  marker.header.frame_id = "head_pan";
+  marker.id = 1;
+  marker.type = 2; 
+  marker.action = 0;
+  marker.x = pan_point.x();
+  marker.y = pan_point.y();
+  marker.z = pan_point.z();
+  marker.yaw = 0;
+  marker.pitch = 0;
+  marker.roll = 0.0;
+  marker.xScale = 0.1;
+  marker.yScale = 0.1;
+  marker.zScale = 0.1;
+  marker.alpha = 255;
+  marker.r = 255;
+  marker.g = 0;
+  marker.b = 0;
+  node_->publish("visualizationMarker", marker ); 
+
+
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6411
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6411&view=rev
Author:   mmwise
Date:     2008-11-07 23:06:48 +0000 (Fri, 07 Nov 2008)

Log Message:
-----------
servoing controller test

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/head_servoing_controller.h	2008-11-07 23:06:48 UTC (rev 6411)
@@ -0,0 +1,229 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Melonee Wise <mwise@...>
+
+#pragma once
+
+// ROS stuff
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+// Controllers
+#include <mechanism_model/controller.h>
+#include <mechanism_model/joint.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+
+// Services
+#include <robot_msgs/JointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
+#include <std_msgs/PointStamped.h>
+#include <std_msgs/VisualizationMarker.h>
+
+// Math utils
+#include <math_utils/angles.h>
+#include <math.h>
+#include <tf/transform_listener.h>
+
+namespace controller
+{
+// The maximum number of joints expected in a head.
+static const int MAX_HEAD_JOINTS = 2;
+
+/***************************************************/
+/*! \class controller::HeadServoingController
+    \brief Head Pan Tilt Controller
+
+    This class closes the loop around the joint angles.
+
+    Example config:<br>
+
+    <controller name="head_controller" type="HeadServoingControllerNode"><br>
+      <listen_topic name="head_commands" /><br>
+
+      <controller name="head_pan_controller" topic="head_pan_controller" type="JointVelocityController"><br>
+        <joint name="head_pan_joint" ><br>
+          <pid p="1.5" d="0.1" i="0.3" iClamp="0.2" /><br>
+        </joint><br>
+      </controller><br>
+
+      <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointVelocityController"><br>
+        <joint name="head_tilt_joint" ><br>
+          <pid p="0.8" d="0.05" i="0.1" iClamp="0.1" /><br>
+        </joint><br>
+      </controller><br>
+    </controller> <br>
+*/
+/***************************************************/
+
+class HeadServoingController : public Controller
+{
+public:
+
+  /*!
+   * \brief Default Constructor of the HeadServoingController class.
+   *
+   */
+  HeadServoingController();
+
+  /*!
+   * \brief Destructor of the HeadServoingController class.
+   */
+  virtual ~HeadServoingController();
+
+  /*!
+   * \brief Functional way to initialize limits and gains.
+   *
+   */
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  /*!
+   * \brief Sets goals for some joint, specified as a pair of (joint values, joint names).
+   *
+   * \param j_values the positions of the joints.
+   * \param j_names the names of the joints.
+   */
+  void setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names);
+
+  /*!
+   * \brief Returns the commanded joint poisitions, specified as a pair of (joint values, joint names).
+   *
+   * \param cmd names and positions.
+   */
+  void getJointCmd(robot_msgs::JointCmd & cmd) const;
+
+  /*!
+   * \brief Returns the position of the controller in the vector.
+   *
+   * \param name the name of the joint being controlled by the controller (i.e. head_pan_joint).
+   */
+  int getJointControllerByName(std::string name);
+
+  /*!
+   * \brief Issues commands to the joint.
+   */
+  virtual void update(void); // Real time safe.
+
+  unsigned int num_joints_; /**< Number of joints. */
+  controller::JointVelocityController* getJointVelocityControllerByName(std::string name); /**< Joints we're controlling. */
+  std::vector<JointVelocityController *> joint_velocity_controllers_; /**< Vector of the joint controllers. */
+
+
+private:
+
+  double last_time_;            /**< The last time. */
+  double max_velocity_;
+  std::vector<double> set_pts_; /**< The vector of joint set_pts. */
+  mechanism::Robot* robot_;     /**< The robot we're controlling. */
+
+  /*!
+   * \brief Issues commands to the joint.
+   */
+  void updateJointControllers(void);
+
+};
+
+/***************************************************/
+/*! \class controller::HeadServoingControllerNode
+    \brief Head Pan Tilt Controller Node
+
+    This class sets the joint angles given a point or
+    joint angles.
+
+*/
+/***************************************************/
+
+class HeadServoingControllerNode : public Controller
+{
+  public:
+
+    HeadServoingControllerNode();
+    ~HeadServoingControllerNode();
+
+    void update();
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * \brief Sets a command for all the joints managed by the controller at once.
+     *
+     * \param joint_cmds_ (names, positions)
+     */
+    void setJointCmd();
+
+    /*!
+     * \brief Gets the commands for all the joints managed by the controller at once.
+     *
+     * \param req
+     * \param resp (positions)
+     */
+    bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+                     robot_srvs::GetJointCmd::response &resp);
+    /*!
+     * \brief This points the head at a specified point from a given frame.
+     *
+     * \param head_track_point_ (header, point)
+     */
+    void headTrackPoint();
+    /*!
+     * \brief This points the given frame at a specified point from the given frame.
+     *
+     * \param frame_track_point_ (header, point)
+     */
+    void frameTrackPoint();
+
+  private:
+
+    //node stuff
+    std::string service_prefix_;                         /**< The service name. */
+    ros::node *node_;                                    /**< The node. */
+    AdvertisedServiceGuard guard_get_command_array_;     /**< Makes sure the advertise goes down neatly. */
+    SubscriptionGuard guard_set_command_array_;          /**< Makes sure the subscription goes down neatly. */
+    SubscriptionGuard guard_head_track_point_;           /**< Makes sure the subscription goes down neatly. */
+    SubscriptionGuard guard_frame_track_point_;           /**< Makes sure the subscription goes down neatly. */
+
+    //msgs
+    std_msgs::PointStamped head_track_point_;          /**< The point from the subscription. */
+    std_msgs::PointStamped frame_track_point_;         /**< The point from the subscription. */
+    robot_msgs::JointCmd joint_cmds_;                  /**< The joint commands from the subscription.*/
+
+    //controller
+    HeadServoingController *c_;                   /**< The controller. */
+
+    //tf
+    tf::TransformListener TF;                    /**< The transform for converting from point to head and tilt frames. */
+
+};
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/head_servoing_controller.cpp	2008-11-07 23:06:48 UTC (rev 6411)
@@ -0,0 +1,373 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Melonee Wise <mwise@...>
+
+#include "pr2_mechanism_controllers/head_servoing_controller.h"
+
+using namespace controller;
+using namespace std;
+
+
+ROS_REGISTER_CONTROLLER(HeadServoingController);
+
+HeadServoingController::HeadServoingController():num_joints_(0),last_time_(0)
+{
+}
+
+HeadServoingController::~HeadServoingController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
+    delete joint_velocity_controllers_[i];
+  }
+}
+
+bool HeadServoingController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  robot_ = robot->model_;
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    JointVelocityController * jvc = new JointVelocityController();
+    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointVelocityController"));
+    joint_velocity_controllers_.push_back(jvc);
+
+    if(!jvc->initXml(robot, elt))
+      return false;
+
+    elt = elt->NextSiblingElement("controller");
+  }
+  
+  TiXmlElement *cd = elt->NextSiblingElement("controller_defaults");
+  if (cd)
+  {
+
+    double max_velocity = atof(cd->Attribute("max_velocity"));
+    max_velocity_ = max_velocity;
+  }
+  else
+  {
+    fprintf(stderr, "HeadServoingController's config did not specify the default control parameters.\n");
+  }
+
+  num_joints_ = joint_velocity_controllers_.size();
+
+  fprintf(stderr,"HeadServoingController:: num_joints_: %d\n",num_joints_);
+
+  
+  set_pts_.resize(num_joints_);
+  last_time_ = robot->hw_->current_time_;
+
+  return true;
+}
+
+void HeadServoingController::setJointCmd(const std::vector<double> &j_values, const std::vector<std::string> & j_names)
+{
+  assert(j_values.size() == j_names.size());
+  for(uint i = 0; i < j_values.size(); ++i)
+  {
+    const std::string & name = j_names[i];
+    const int id = getJointControllerByName(name);
+    assert(id>=0);
+    if(id >= 0)
+    {
+      set_pts_[id] = j_values[i];
+    }
+  }
+}
+
+void HeadServoingController::getJointCmd(robot_msgs::JointCmd & cmd) const
+{
+  const unsigned int n = joint_velocity_controllers_.size();
+  cmd.set_names_size(n);
+  for(unsigned int i=0; i<n; ++i)
+      cmd.names[i] = joint_velocity_controllers_[i]->getJointName();
+
+  cmd.set_positions_vec(set_pts_);
+}
+
+
+controller::JointVelocityController* HeadServoingController::getJointVelocityControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_velocity_controllers_[i]->getJointName() == name)
+    {
+      return joint_velocity_controllers_[i];
+    }
+  }
+    return NULL;
+}
+
+
+int HeadServoingController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_velocity_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+void HeadServoingController::update(void)
+{
+  double error;
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
+    math_utils::shortest_angular_distance_with_limits(set_pts_[i], joint_velocity_controllers_[i]->joint_state_->position_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_velocity_controllers_[i]->joint_state_->joint_->joint_limit_max_,error);
+    error=(error<max_velocity_)?error:max_velocity_;
+    joint_velocity_controllers_[i]->setCommand(error);
+  }
+
+  updateJointControllers();
+}
+
+void HeadServoingController::updateJointControllers(void)
+{
+
+  for(unsigned int i=0;i<num_joints_;++i)
+  {
+    if (!joint_velocity_controllers_[i]->joint_state_->calibrated_)
+      return;  // joints are not calibrated
+
+    joint_velocity_controllers_[i]->update();
+  }
+}
+
+//------ Head controller node --------
+
+ROS_REGISTER_CONTROLLER(HeadServoingControllerNode)
+
+HeadServoingControllerNode::HeadServoingControllerNode()
+: Controller(), node_(ros::node::instance()), TF(*ros::node::instance(),false, 10000000000ULL)
+{
+  c_ = new HeadServoingController();
+}
+
+HeadServoingControllerNode::~HeadServoingControllerNode()
+{
+  delete c_;
+}
+
+void HeadServoingControllerNode::update()
+{
+  c_->update();
+}
+
+bool HeadServoingControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  assert(node_);
+  service_prefix_ = config->Attribute("name");
+
+  // Parses subcontroller configuration
+  if(!c_->initXml(robot, config))
+    return false;
+  //suscriptions
+  node_->subscribe(service_prefix_ + "/head_track_point", head_track_point_, &HeadServoingControllerNode::headTrackPoint, this, 1);
+  guard_head_track_point_.set(service_prefix_ + "/head_track_point");
+  node_->subscribe(service_prefix_ + "/frame_track_point", frame_track_point_, &HeadServoingControllerNode::frameTrackPoint, this, 1);
+  guard_frame_track_point_.set(service_prefix_ + "/frame_track_point");
+  //services
+  node_->advertise<std_msgs::VisualizationMarker>( "visualizationMarker", 0 );
+  return true;
+
+}
+
+void HeadServoingControllerNode::headTrackPoint()
+{
+  std::vector<double> pos;
+  std::vector<std::string> names;
+
+  tf::Stamped<tf::Point> point;
+  point.setX(head_track_point_.point.x);
+  point.setY(head_track_point_.point.y);
+  point.setZ(head_track_point_.point.z);
+  point.stamp_ = head_track_point_.header.stamp;
+  point.frame_id_ = head_track_point_.header.frame_id;
+
+  tf::Stamped<tf::Point> pan_point;
+
+  try{
+    TF.transformPoint("head_pan",point, pan_point);
+  }
+  catch(tf::TransformException& ex){
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+  int id = c_->getJointControllerByName("head_pan_joint");
+  assert(id>=0);
+  double meas_pan_angle = c_->joint_velocity_controllers_[id]->joint_state_->position_;
+  double head_pan_angle= meas_pan_angle + atan2(pan_point.y(), pan_point.x());
+
+  names.push_back("head_pan_joint");
+  pos.push_back(head_pan_angle);
+
+  tf::Stamped<tf::Point> tilt_point;
+
+  try{
+    TF.transformPoint("head_tilt",point,tilt_point);
+  }
+  catch(tf::TransformException& ex){
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+  id = c_->getJointControllerByName("head_tilt_joint");
+  assert(id>=0);
+  double meas_tilt_angle= c_->joint_velocity_controllers_[id]->joint_state_->position_;
+  double head_tilt_angle= meas_tilt_angle + atan2(-tilt_point.z(), tilt_point.x());
+
+  names.push_back("head_tilt_joint");
+  pos.push_back(head_tilt_angle);
+
+  c_->setJointCmd(pos,names);
+
+}
+
+
+void HeadServoingControllerNode::frameTrackPoint()
+{
+  std::vector<double> pos;
+  std::vector<std::string> names;
+  tf::Stamped<tf::Transform> frame;
+
+  tf::Stamped<tf::Point> point;
+  point.setX(frame_track_point_.point.x);
+  point.setY(frame_track_point_.point.y);
+  point.setZ(frame_track_point_.point.z);
+  point.stamp_ = frame_track_point_.header.stamp;
+  point.frame_id_ = frame_track_point_.header.frame_id;
+
+  try
+  {
+    TF.lookupTransform(point.frame_id_,"head_pan",ros::Time(0.0),frame);
+  }
+  catch(tf::TransformException& ex)
+  {
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+  tf::Stamped<tf::Point> pan_point;
+
+  try
+  {
+    TF.transformPoint("head_pan",point, pan_point);
+  }
+  catch(tf::TransformException& ex)
+  {
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+  int id = c_->getJointControllerByName("head_pan_joint");
+  assert(id>=0);
+  double radius = pow(pan_point.x(),2)+pow(pan_point.y(),2);
+  double x_intercept = sqrt(radius-pow(frame.getOrigin().y(),2));
+  double delta_theta = atan2(pan_point.y(), pan_point.x())-atan2(frame.getOrigin().y(),x_intercept);
+  double meas_pan_angle = c_->joint_velocity_controllers_[id]->joint_state_->position_;
+  double head_pan_angle= meas_pan_angle + delta_theta;
+
+  names.push_back("head_pan_joint");
+  pos.push_back(head_pan_angle);
+
+  tf::Stamped<tf::Point> tilt_point;
+  try{
+    TF.transformPoint("head_tilt",point,tilt_point);
+  }
+  catch(tf::TransformException& ex){
+    ROS_WARN("Transform Exception %s", ex.what());
+    return;
+  }
+
+
+  id = c_->getJointControllerByName("head_tilt_joint");
+  assert(id>=0);
+  radius = pow(tilt_point.x(),2)+pow(tilt_point.y(),2);
+  x_intercept = sqrt(radius-pow(frame.getOrigin().z(),2));
+  delta_theta = atan2(-tilt_point.z(), tilt_point.x())-atan2(frame.getOrigin().z(),x_intercept);
+  double meas_tilt_angle= c_->joint_velocity_controllers_[id]->joint_state_->position_;
+  double head_tilt_angle= meas_tilt_angle + delta_theta;
+
+  names.push_back("head_tilt_joint");
+  pos.push_back(head_tilt_angle);
+
+  c_->setJointCmd(pos,names);
+
+  std_msgs::VisualizationMarker marker;
+  marker.header.frame_id = "stereo";
+  marker.id = 0;
+  marker.type = 0; 
+  marker.action = 0;
+  marker.x = 0;
+  marker.y = 0;
+  marker.z = 0;
+  marker.yaw = 0;
+  marker.pitch = 0;
+  marker.roll = 0.0;
+  marker.xScale = 2;
+  marker.yScale = 0.05;
+  marker.zScale = 0.05;
+  marker.alpha = 255;
+  marker.r = 0;
+  marker.g = 255;
+  marker.b = 0;
+  node_->publish("visualizationMarker", marker );
+
+  marker.header.frame_id = "head_pan";
+  marker.id = 1;
+  marker.type = 2; 
+  marker.action = 0;
+  marker.x = pan_point.x();
+  marker.y = pan_point.y();
+  marker.z = pan_point.z();
+  marker.yaw = 0;
+  marker.pitch = 0;
+  marker.roll = 0.0;
+  marker.xScale = 0.1;
+  marker.yScale = 0.1;
+  marker.zScale = 0.1;
+  marker.alpha = 255;
+  marker.r = 255;
+  marker.g = 0;
+  marker.b = 0;
+  node_->publish("visualizationMarker", marker ); 
+
+
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6663
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6663&view=rev
Author:   vijaypradeep
Date:     2008-11-13 04:35:11 +0000 (Thu, 13 Nov 2008)

Log Message:
-----------
Adding first cut stub of a trajectory based laser_controller.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-11-13 04:32:23 UTC (rev 6662)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-11-13 04:35:11 UTC (rev 6663)
@@ -8,6 +8,7 @@
 rospack_add_library( pr2_mechanism_controllers
   src/base_controller.cpp
   src/laser_scanner_controller.cpp
+  src/laser_scanner_traj_controller.cpp
   src/laser_scanner_velocity_controller.cpp
   src/arm_position_controller.cpp
   src/arm_velocity_controller.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2008-11-13 04:35:11 UTC (rev 6663)
@@ -0,0 +1,112 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
+
+#include <misc_utils/realtime_publisher.h>
+
+// Messages
+#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+
+// Services
+#include <robot_mechanism_controllers/SetCommand.h>
+#include <robot_mechanism_controllers/GetCommand.h>
+#include <pr2_mechanism_controllers/SetProfile.h>
+
+#include "rosthread/mutex.h"
+#include "trajectory/trajectory.h"
+
+namespace controller
+{
+
+class LaserScannerTrajController : public Controller
+{
+public:
+  LaserScannerTrajController() ;
+  ~LaserScannerTrajController() ;
+  
+  
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+  virtual void update();
+
+  void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp ) ;
+  void sampleTrajectory(double time_from_start) ;
+
+private:
+  mechanism::RobotState *robot_ ;
+  mechanism::JointState *joint_state_ ;                                 // Need this to check the calibrated flag on the joint
+  
+  ros::thread::mutex traj_lock_ ;                                       // Mutex for traj_
+  trajectory::Trajectory traj_ ;                                        // Stores the current trajectory being executed
+
+  JointPositionSmoothController joint_position_controller_ ;            // The PID position controller that is doing all the under-the-hood controls stuff
+  
+  double traj_start_time_ ;
+  
+  unsigned int update_count_ ;
+  
+};
+
+class LaserScannerTrajControllerNode : public Controller
+{
+public:
+  LaserScannerTrajControllerNode() ;
+  ~LaserScannerTrajControllerNode() ;
+
+  void update() ;
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config) ;
+
+  // Message Callbacks
+  void setCommand() ;
+
+private:
+  LaserScannerTrajController *c_ ;
+  std::string service_prefix_ ;
+
+  ros::node *node_ ;
+
+  std_msgs::Float64 cmd_;                      //! \todo Change interface to accept arbitrary trajectories
+};
+
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-11-13 04:32:23 UTC (rev 6662)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2008-11-13 04:35:11 UTC (rev 6663)
@@ -18,6 +18,8 @@
   <depend package="math_utils" />
   <depend package="robot_kinematics" />
   <depend package="rosout" />
+  <depend package="trajectory" />
+  <depend package="rosthread" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2008-11-13 04:35:11 UTC (rev 6663)
@@ -0,0 +1,258 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+#include <algorithm>
+#include <pr2_mechanism_controllers/laser_scanner_traj_controller.h>
+#include <math_utils/angles.h>
+
+#include <math.h>
+
+using namespace std;
+using namespace controller;
+
+ROS_REGISTER_CONTROLLER(LaserScannerTrajController)
+
+LaserScannerTrajController::LaserScannerTrajController() : traj_(1)
+{
+  update_count_ = 0 ;
+}
+
+LaserScannerTrajController::~LaserScannerTrajController()
+{
+
+}
+
+bool LaserScannerTrajController::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  if (!robot || !config)
+    return false ;
+  robot_ = robot ;
+
+  // Look through XML to grab the joint name
+  TiXmlElement *j = config->FirstChildElement("joint") ;
+  if (!j)
+    return false ;
+
+  const char *jn = j->Attribute("name") ;
+
+  if (!jn)
+    return false ;  
+  std::string joint_name = jn ;
+  
+  joint_state_ = robot_->getJointState(joint_name) ;  // Need joint state to check calibrated flag
+  
+  joint_position_controller_.initXml(robot, config) ; //Pass down XML snippet to encapsulated joint_position_controller_
+  return true ;
+}
+
+void LaserScannerTrajController::update()
+{
+  if (!joint_state_->calibrated_)
+    return;
+
+  double time = robot_->hw_->current_time_ ;
+  
+  if (!traj_lock_.trylock())
+  {
+    const double traj_duration = traj_.getTotalTime() ;
+    if (traj_duration > 1e-6)                                   // Short trajectories make the mod_time calculation unstable
+    {
+      double time_from_start = time - traj_start_time_ ;
+      double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
+      
+      trajectory::Trajectory::TPoint sampled_point ;
+      sampled_point.dimension_ = 1 ;
+      sampled_point.q_.resize(1) ;
+      sampled_point.qdot_.resize(1) ;
+      int result ;
+      
+      result = traj_.sample(sampled_point, mod_time) ;
+      if (result > 0)
+      {
+        joint_position_controller_.setCommand(sampled_point.q_[0]) ;
+        joint_position_controller_.update() ;
+
+        if (update_count_ % 10 == 0)
+          printf("%f %f\n", mod_time, sampled_point.q_[0]) ;
+        update_count_++ ;
+      }
+  
+    }
+    traj_lock_.unlock() ;
+  }
+}
+
+void LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
+{
+  while (!traj_lock_.trylock())
+    usleep(100) ;
+
+  vector<double> max_rates ;
+  max_rates.push_back(max_rate) ;
+  vector<double> max_accs ;
+  max_accs.push_back(max_acc) ;
+
+  traj_.setMaxRates(max_rates) ;
+  traj_.setMaxAcc(max_accs) ;
+  traj_.setInterpolationMethod(interp) ;
+  
+  //for (unsigned int i=0; i<traj_points.
+  
+  traj_.setTrajectory(traj_points) ;
+
+  traj_start_time_ = robot_->hw_->current_time_ ;
+  
+  traj_lock_.unlock() ;
+}
+
+ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
+LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance())
+{
+  c_ = new LaserScannerTrajController();
+}
+
+LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
+{
+  node_->unadvertise_service(service_prefix_ + "/set_command");
+  delete c_;
+}
+
+void LaserScannerTrajControllerNode::update()
+{
+  c_->update();
+}
+
+
+void LaserScannerTrajControllerNode::setCommand()
+{
+  if (cmd_.data >= -.5 && cmd_.data <= .5)
+  {
+    printf("TRAJECTORY 0\n") ;
+    
+    std::vector<trajectory::Trajectory::TPoint> tpoints ;
+    
+    trajectory::Trajectory::TPoint cur_point(1) ;
+    
+    cur_point.dimension_ = 1 ;
+    
+    cur_point.q_[0] = 0 ;
+    cur_point.time_ = 0.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = 0 ;
+    cur_point.time_ = 50.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = 0 ;
+    cur_point.time_ = 100.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    double max_rate = 1 ;
+    double max_acc = 1 ;
+    
+    c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;    
+  }
+  else if (cmd_.data >= -52.1 && cmd_.data <= -51.9)
+  {
+    printf("TRAJECTORY -52\n") ;
+    
+    std::vector<trajectory::Trajectory::TPoint> tpoints ;
+    
+    trajectory::Trajectory::TPoint cur_point(1) ;
+    
+    cur_point.dimension_ = 1 ;
+    
+    cur_point.q_[0] = .5 ;
+    cur_point.time_ = 0.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = -.5 ;
+    cur_point.time_ = 5.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = .5 ;
+    cur_point.time_ = 10.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    double max_rate = 1 ;
+    double max_acc = 1 ;
+    
+    c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;
+  }
+  else if (cmd_.data >= -53.1 && cmd_.data <= -52.9)
+  {
+    printf("TRAJECTORY -53\n") ;
+    
+    std::vector<trajectory::Trajectory::TPoint> tpoints ;
+    
+    trajectory::Trajectory::TPoint cur_point(1) ;
+    
+    cur_point.dimension_ = 1 ;
+    
+    cur_point.q_[0] = .5 ;
+    cur_point.time_ = 0.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = -.5 ;
+    cur_point.time_ = 5.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    cur_point.q_[0] = .5 ;
+    cur_point.time_ = 10.0 ;
+    tpoints.push_back(cur_point) ;
+    
+    double max_rate = 1 ;
+    double max_acc = 1 ;
+    
+    c_->setTrajectory(tpoints, max_rate, max_acc, "blended_linear") ;
+  }
+
+}
+
+bool LaserScannerTrajControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  printf("Starting to init XML\n") ;
+  
+  service_prefix_ = config->Attribute("name");
+
+  if (!c_->initXml(robot, config))
+  {
+    printf("Error initialized XML\n") ;
+    return false;
+  }
+  node_->subscribe(service_prefix_ + "/set_command", cmd_, &LaserScannerTrajControllerNode::setCommand, this, 1);
+
+  printf("Done with initing XML\n") ;
+  
+  return true;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6753
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6753&view=rev
Author:   sachinchitta
Date:     2008-11-14 21:26:01 +0000 (Fri, 14 Nov 2008)

Log Message:
-----------
Adding message for odom calibration

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-11-14 21:20:29 UTC (rev 6752)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-11-14 21:26:01 UTC (rev 6753)
@@ -44,6 +44,7 @@
 // Services
 #include <pr2_mechanism_controllers/SetBaseCommand.h>
 #include <pr2_mechanism_controllers/GetBaseCommand.h>
+#include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
 
 #include <libTF/Pose3D.h>
 #include <urdf/URDF.h>
@@ -254,6 +255,10 @@
 
     double wheel_radius_; /** radius of the wheel (filled in from urdf robot model) */
 
+    double wheel_radius_multiplier_front_;
+
+    double wheel_radius_multiplier_rear_;
+
     private:
 
     bool new_cmd_available_; /** true when new command received by node */
@@ -430,6 +435,12 @@
     bool getCommand(pr2_mechanism_controllers::GetBaseCommand::request &req,
                     pr2_mechanism_controllers::GetBaseCommand::response &resp);
 
+    bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+                                                      pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+
+    bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+                                                      pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+
     /*
      * \brief callback function for setting the desired velocity using a topic 
      */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-14 21:20:29 UTC (rev 6752)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-14 21:26:01 UTC (rev 6753)
@@ -91,6 +91,9 @@
 
    new_cmd_available_ = false;
 
+   wheel_radius_multiplier_front_ = 1.0;
+   wheel_radius_multiplier_rear_  = 1.0;
+
    pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
@@ -502,7 +505,7 @@
 
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
     wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/wheel_radius_;
+    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/(wheel_radius_*wheel_radius_multiplier_front_);
   }
 }
 
@@ -593,7 +596,7 @@
   for(int i = 0; i < num_wheels_; i++) {
     caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/wheel_radius_;
+    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/(wheel_radius_*wheel_radius_multiplier_front_);
 
     steer_angle = base_wheels_[i].parent_->joint_state_->position_;
     A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
@@ -743,6 +746,9 @@
 {
   node->unadvertise_service(service_prefix + "/set_command");
   node->unadvertise_service(service_prefix + "/get_actual");
+  node->unadvertise_service(service_prefix + "/set_wheel_radius_multiplier");
+  node->unadvertise_service(service_prefix + "/get_wheel_radius_multiplier");
+
   node->unsubscribe("cmd_vel");
 
   publisher_->stop();
@@ -849,6 +855,27 @@
   return true;
 }
 
+bool BaseControllerNode::getWheelRadiusMultiplier(
+  pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+  pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
+{
+  resp.wheel_radius_multiplier_front = c_->wheel_radius_multiplier_front_;
+  resp.wheel_radius_multiplier_rear  = c_->wheel_radius_multiplier_rear_;
+
+  return true;
+}
+
+bool BaseControllerNode::setWheelRadiusMultiplier(
+  pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+  pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
+{
+  c_->wheel_radius_multiplier_front_ = req.wheel_radius_multiplier_front;
+  c_->wheel_radius_multiplier_rear_  = req.wheel_radius_multiplier_rear;
+
+  return true;
+}
+
+
 bool BaseControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
 {
   service_prefix = config->Attribute("name");
@@ -860,6 +887,8 @@
 
   node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
   node->advertise_service(service_prefix + "/get_command", &BaseControllerNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+
+  node->advertise_service(service_prefix + "/set_command", &BaseControllerNode::setCommand, this);
   node->subscribe("cmd_vel", baseVelMsg, &BaseControllerNode::CmdBaseVelReceived, this,1);
 
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6770
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6770&view=rev
Author:   sachinchitta
Date:     2008-11-14 22:54:35 +0000 (Fri, 14 Nov 2008)

Log Message:
-----------
cleaned up parameter server lookup and services

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-14 22:40:11 UTC (rev 6769)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-14 22:54:35 UTC (rev 6770)
@@ -91,9 +91,6 @@
 
    new_cmd_available_ = false;
 
-   wheel_radius_multiplier_front_ = 1.0;
-   wheel_radius_multiplier_rear_  = 1.0;
-
    pthread_mutex_init(&base_controller_lock_,NULL);
 }
 
@@ -506,7 +503,7 @@
 
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
     wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
-    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/(wheel_radius_*wheel_radius_multiplier_front_);
+    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/(wheel_radius_);
   }
 }
 
@@ -597,7 +594,7 @@
   for(int i = 0; i < num_wheels_; i++) {
     caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
     wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
-    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/(wheel_radius_*wheel_radius_multiplier_front_);
+    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/(wheel_radius_);
 
     steer_angle = base_wheels_[i].parent_->joint_state_->position_;
     A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
@@ -860,8 +857,9 @@
   pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
   pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
 {
-  resp.wheel_radius_multiplier_front = c_->wheel_radius_multiplier_front_;
-  resp.wheel_radius_multiplier_rear  = c_->wheel_radius_multiplier_rear_;
+  double param_multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",param_multiplier,1.0);
+  resp.radius_multiplier = param_multiplier;
 
   return true;
 }
@@ -870,13 +868,14 @@
   pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
   pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
 {
-  c_->wheel_radius_multiplier_front_ = req.wheel_radius_multiplier_front;
-  c_->wheel_radius_multiplier_rear_  = req.wheel_radius_multiplier_rear;
+  double calibration_multiplier = req.radius_multiplier;
+  ROS_INFO("Received radius multiplier %f ",calibration_multiplier); 
+  c_->wheel_radius_ *= calibration_multiplier;
 
-  ROS_INFO("Received radius request %f ",req.wheel_radius_multiplier_front); 
-  ROS_INFO("Set radius request %f ",c_->wheel_radius_multiplier_front_ ); 
+  double param_multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",param_multiplier,1.0);
 
-  node->set_param("base_controller/wheel_radius_multiplier",c_->wheel_radius_multiplier_front_);
+  node->set_param("base_controller/wheel_radius_multiplier",param_multiplier*calibration_multiplier);
 
   return true;
 }
@@ -914,14 +913,11 @@
     delete transform_publisher_ ;
   transform_publisher_ = new misc_utils::RealtimePublisher <rosTF::TransformArray> ("TransformArray", 5) ;
 
-  double multiplier;
 
   node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
-  node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
-  c_->wheel_radius_multiplier_rear_ = c_->wheel_radius_multiplier_front_;
 
-  ROS_INFO("Getting value from param server: %f", c_->wheel_radius_multiplier_front_);
-
+  double multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
   c_->wheel_radius_ = c_->wheel_radius_*multiplier;
 
   transform_publisher_->msg_.set_eulers_size(NUM_TRANSFORMS);

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv	2008-11-14 22:40:11 UTC (rev 6769)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/WheelRadiusMultiplier.srv	2008-11-14 22:54:35 UTC (rev 6770)
@@ -1,5 +1,3 @@
-float64 wheel_radius_multiplier_front
-float64 wheel_radius_multiplier_rear
+float64 radius_multiplier
 ---
-float64 wheel_radius_multiplier_front
-float64 wheel_radius_multiplier_rear
+float64 radius_multiplier


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 6847
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=6847&view=rev
Author:   sachinchitta
Date:     2008-11-18 00:02:29 +0000 (Tue, 18 Nov 2008)

Log Message:
-----------
Fixing the wheels so they will stay at the steering angle specified by the last non-zero command. Also removed duplicate calculations in odometry calculations. 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-11-17 23:36:42 UTC (rev 6846)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-11-18 00:02:29 UTC (rev 6847)
@@ -365,6 +365,8 @@
 
     std::vector<double> steer_angle_actual_; /** vector of actual caster steer angles */
 
+    std::vector<double> steer_angle_stored_; /** vector of stored caster steer angles */
+
     std::vector<double> wheel_speed_actual_; /** vector of actual wheel speeds */
 
     double last_time_; /** time corresponding to when update was last called */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-17 23:36:42 UTC (rev 6846)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-11-18 00:02:29 UTC (rev 6847)
@@ -39,6 +39,10 @@
 
 #define NUM_TRANSFORMS 2
 #define EPS 1e-5
+#define CMD_VEL_TRANS_EPS 1e-5
+#define CMD_VEL_ROT_EPS 1e-5
+
+
 using namespace ros;
 using namespace std;
 using namespace controller;
@@ -228,6 +232,7 @@
       base_object.local_id_ = num_casters_;
       base_casters_.push_back(base_object);
       steer_angle_actual_.push_back(0);
+      steer_angle_stored_.push_back(0);
       steer_velocity_desired_.push_back(0);
       num_casters_++;
 //      cout << "base_casters" << "::  " << base_object;
@@ -457,12 +462,20 @@
 
   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
   double error_steer(0.0),error_steer_m_pi(0.0);
-
+  double trans_vel = sqrt(cmd_vel_.x * cmd_vel_.x + cmd_vel_.y * cmd_vel_.y);
   for(int i=0; i < num_casters_; i++)
   {
     result = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
 
-    steer_angle_desired = atan2(result.y,result.x);
+    if( trans_vel < CMD_VEL_TRANS_EPS && fabs(cmd_vel_.z) < CMD_VEL_ROT_EPS)
+    {
+      steer_angle_desired = steer_angle_stored_[i];
+    }
+    else
+    {
+      steer_angle_desired = atan2(result.y,result.x);
+      steer_angle_stored_[i] = steer_angle_desired;
+    }
     steer_angle_desired_m_pi = normalize_angle(steer_angle_desired+M_PI);
 
     error_steer = shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
@@ -474,6 +487,7 @@
       steer_angle_desired = steer_angle_desired_m_pi;
     }
     steer_velocity_desired_[i] =  -kp_speed_*error_steer;
+
   }
 }
 
@@ -597,17 +611,23 @@
     wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/(wheel_radius_);
 
     steer_angle = base_wheels_[i].parent_->joint_state_->position_;
-    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
-    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
-  }
 
-  for(int i = 0; i < num_wheels_; i++) {
-    C.element(i*2, 0)   = 1;
-    C.element(i*2, 1)   = 0;
-    C.element(i*2, 2)   = -base_wheels_position_[i].y;
-    C.element(i*2+1, 0) = 0;
-    C.element(i*2+1, 1) = 1;
-    C.element(i*2+1, 2) =  base_wheels_position_[i].x;
+//    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
+//    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
+
+    A.element(i*2,0)   = wheel_radius_*wheel_speed;
+    A.element(i*2+1,0) = 0;
+
+
+//  }
+//  for(int i = 0; i < num_wheels_; i++) {
+
+    C.element(i*2, 0)   = cos(steer_angle);
+    C.element(i*2, 1)   = sin(steer_angle);
+    C.element(i*2, 2)   = -cos(steer_angle) * base_wheels_position_[i].y + sin(steer_angle) *  base_wheels_position_[i].x;
+    C.element(i*2+1, 0)   = -sin(steer_angle);
+    C.element(i*2+1, 1)   = cos(steer_angle);
+    C.element(i*2+1, 2)   = sin(steer_angle) * base_wheels_position_[i].y + cos(steer_angle) *  base_wheels_position_[i].x;
   }
   //   D = pseudoInverse(C)*A;
   D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7152
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7152&view=rev
Author:   rob_wheeler
Date:     2008-11-21 19:53:21 +0000 (Fri, 21 Nov 2008)

Log Message:
-----------
Fix compiler warnings

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp	2008-11-21 19:48:01 UTC (rev 7151)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/gripper_calibration_controller.cpp	2008-11-21 19:53:21 UTC (rev 7152)
@@ -174,7 +174,6 @@
 bool GripperCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
 {
   robot_ = robot;
-  ros::node *node = ros::node::instance();
 
   std::string topic = config->Attribute("name") ? config->Attribute("name") : "";
   if (topic == "")

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	2008-11-21 19:48:01 UTC (rev 7151)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	2008-11-21 19:53:21 UTC (rev 7152)
@@ -67,7 +67,7 @@
 {
   /*********** Initialize ROS  ****************/
   ros::init(argc,argv);
-  ros::node *node = new ros::node("test_arm_dynamics_controller"); 
+  //ros::node *node = new ros::node("test_arm_dynamics_controller"); 
 
   signal(SIGINT,  finalize);
   signal(SIGQUIT, finalize);

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-11-21 19:48:01 UTC (rev 7151)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_base_controller.cpp	2008-11-21 19:53:21 UTC (rev 7152)
@@ -26,9 +26,7 @@
 
   /*********** Initialize ROS  ****************/
   ros::init(argc,argv);
-  ros::node *node = new ros::node("test_base_controller"); 
 
-
   /*********** Load the robot model and state file ************/
   char *xml_robot_file = argv[1];
   TiXmlDocument xml(xml_robot_file);   // Load robot description


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7224
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7224&view=rev
Author:   stuglaser
Date:     2008-11-24 22:33:21 +0000 (Mon, 24 Nov 2008)

Log Message:
-----------
CasterControllerNode now waits for calibration before commanding the caster

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h	2008-11-24 22:12:11 UTC (rev 7223)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h	2008-11-24 22:33:21 UTC (rev 7224)
@@ -73,8 +73,9 @@
   double getSteerPosition() { return caster_->position_; }
   double getSteerVelocity() { return caster_->velocity_; }
 
+  mechanism::JointState *caster_;
+
 private:
-  mechanism::JointState *caster_;
   JointVelocityController caster_vel_, wheel_l_vel_, wheel_r_vel_;
 };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp	2008-11-24 22:12:11 UTC (rev 7223)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp	2008-11-24 22:33:21 UTC (rev 7224)
@@ -172,6 +172,8 @@
 
 void CasterControllerNode::update()
 {
+  if (!c_.caster_->calibrated_)
+    return;
   c_.update();
 }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7322
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7322&view=rev
Author:   sachinchitta
Date:     2008-11-26 02:00:08 +0000 (Wed, 26 Nov 2008)

Log Message:
-----------
Joint trajectory controller. See test/test_arm_trajectory_controller.cpp to see how to address the topic. Also see arm_gazebo/controllers.xml for an xml specification for the controller.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTraj.msg
    pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTrajPoint.msg
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-11-26 01:50:52 UTC (rev 7321)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-11-26 02:00:08 UTC (rev 7322)
@@ -21,6 +21,7 @@
   src/caster_calibration_controller.cpp
   src/wrist_calibration_controller.cpp
   src/gripper_calibration_controller.cpp
+  src/arm_trajectory_controller.cpp
   )
 
 #rospack_add_executable(testBaseController test/test_base_controller.cpp)
@@ -34,3 +35,6 @@
 
 #rospack_add_executable(testOdom test/test_odom.cpp)
 #target_link_libraries(testOdom pr2_mechanism_controllers)
+
+rospack_add_executable(testTraj test/test_arm_trajectory_controller.cpp)
+target_link_libraries(testTraj pr2_mechanism_controllers)

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2008-11-26 02:00:08 UTC (rev 7322)
@@ -0,0 +1,218 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+
+// Services
+#include <pr2_mechanism_controllers/SetJointPosCmd.h>
+#include <pr2_mechanism_controllers/GetJointPosCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointGains.h>
+#include <pr2_mechanism_controllers/GetJointGains.h>
+
+#include <pr2_mechanism_controllers/SetCartesianPosCmd.h>
+#include <pr2_mechanism_controllers/GetCartesianPosCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointTarget.h>
+#include <pr2_mechanism_controllers/JointPosCmd.h>
+
+#include <pr2_mechanism_controllers/JointTraj.h>
+#include <pr2_mechanism_controllers/JointTrajPoint.h>
+
+//Kinematics
+#include <robot_kinematics/robot_kinematics.h>
+#include <trajectory/trajectory.h>
+
+// #include <libTF/Pose3D.h>
+// #include <urdf/URDF.h>
+
+namespace controller
+{
+
+// The maximum number of joints expected in an arm.
+  static const int MAX_ARM_JOINTS = 7;
+
+  class ArmTrajectoryController : public Controller
+  {
+    public:
+
+    /*!
+     * \brief Default Constructor of the JointController class.
+     *
+     */
+    ArmTrajectoryController();
+
+    /*!
+     * \brief Destructor of the JointController class.
+     */
+    virtual ~ArmTrajectoryController();
+
+    /*!
+     * \brief Functional way to initialize limits and gains.
+     */
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * \brief set the joint trajectory for the arm
+     */
+    void setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
+
+    void getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const;
+
+    /*!
+     * \brief Issues commands to the joint. Should be called at regular intervals
+     */
+    virtual void update(void); // Real time safe.
+
+    ros::thread::mutex arm_controller_lock_;
+
+    controller::JointPositionController* getJointControllerByName(std::string name);
+
+    private:
+
+    std::vector<JointPositionController *> joint_position_controllers_;
+
+    trajectory::Trajectory *joint_trajectory_;
+
+    trajectory::Trajectory::TPoint trajectory_point_;
+
+    std::vector<double> joint_cmd_rt_;
+
+    mechanism::Robot* robot_;
+
+    void updateJointControllers(void);
+
+    int getJointControllerPosByName(std::string name);
+
+    // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
+    bool refresh_rt_vals_;
+
+    double trajectory_start_time_;
+
+    int dimension_;
+
+    friend class ArmTrajectoryControllerNode;
+  };
+
+/** @class ArmTrajectoryControllerNode
+ *  @brief ROS interface for the arm controller.
+ *  @author Sachin Chitta <sachinc@...>
+ *
+ *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
+ *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
+ *
+ */
+  class ArmTrajectoryControllerNode : public Controller
+  {
+    public:
+    /*!
+     * \brief Default Constructor
+     *
+     */
+    ArmTrajectoryControllerNode();
+
+    /*!
+     * \brief Destructor
+     */
+    ~ArmTrajectoryControllerNode();
+
+    void update();
+
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /** @brief service that returns the goal of the controller
+     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
+     * @param req
+     * @param resp the response, contains a JointPosCmd message with the goal of the controller
+     * @return
+     */
+    bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::request &req,
+                        pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+
+
+    private:
+
+    /*!
+     * \brief mutex lock for setting and getting ros messages
+     */
+    ros::thread::mutex ros_lock_;
+
+    pr2_mechanism_controllers::JointTraj traj_msg_;
+
+    pr2_mechanism_controllers::JointPosCmd msg_;   //The message used by the ROS callback
+    ArmTrajectoryController *c_;
+
+    /*!
+     * \brief service prefix
+     */
+    std::string service_prefix_;
+
+    /*
+     * \brief save topic name for unsubscribe later
+     */
+    std::string topic_name_;
+
+    /*!
+     * \brief xml pointer to ros topic name
+     */
+    TiXmlElement * topic_name_ptr_;
+
+    /*
+     * \brief pointer to ros node
+     */
+    ros::node * const node_;
+
+
+    /*
+     * \brief receive and set the trajectory in the controller
+     */
+    void CmdTrajectoryReceived();
+
+  };
+
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTraj.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTraj.msg	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTraj.msg	2008-11-26 02:00:08 UTC (rev 7322)
@@ -0,0 +1 @@
+JointTrajPoint[] points
\ No newline at end of file

Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTrajPoint.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTrajPoint.msg	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/JointTrajPoint.msg	2008-11-26 02:00:08 UTC (rev 7322)
@@ -0,0 +1,2 @@
+float64[] positions
+float64 time
\ No newline at end of file

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2008-11-26 02:00:08 UTC (rev 7322)
@@ -0,0 +1,296 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Sachin Chitta <sachinc@...>
+
+#include "pr2_mechanism_controllers/arm_trajectory_controller.h"
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(ArmTrajectoryController);
+
+ArmTrajectoryController::ArmTrajectoryController() :
+  refresh_rt_vals_(false)
+{
+}
+
+ArmTrajectoryController::~ArmTrajectoryController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i<joint_position_controllers_.size();++i)
+    delete joint_position_controllers_[i];
+}
+
+bool ArmTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  ROS_INFO("Initializing trajectory controller");
+
+  robot_ = robot->model_;
+  mechanism::Joint *joint;
+  std::vector<double> joint_velocity_limits;
+  std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
+
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    JointPositionController * jpc = new JointPositionController();
+//    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
+    ROS_INFO("Joint Position Controller: %s , %s",(elt->Attribute("type")),(elt->Attribute("name")));
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPositionController"));
+
+    joint_position_controllers_.push_back(jpc);
+    if(!jpc->initXml(robot, elt))
+      return false;
+
+    joint = (robot->getJointState(jpc->getJointName()))->joint_;
+    if(joint)
+      joint_velocity_limits.push_back(joint->velocity_limit_/5.0);
+
+    elt = elt->NextSiblingElement("controller");
+  }
+
+  ROS_INFO("ArmTrajectoryController:: Done loading controllers");
+
+  elt = config->FirstChildElement("trajectory");
+  ROS_INFO("ArmTrajectoryController:: interpolation type:: %s",elt->Attribute("interpolation"));
+  joint_cmd_rt_.resize(joint_position_controllers_.size()); 
+
+  joint_trajectory_ = new trajectory::Trajectory((int) joint_position_controllers_.size());
+
+  joint_trajectory_->setMaxRates(joint_velocity_limits);
+  joint_trajectory_->setInterpolationMethod(std::string(elt->Attribute("interpolation")));
+
+  trajectory_point_.setDimension((int) joint_position_controllers_.size());
+  dimension_ = (int) joint_position_controllers_.size();
+
+  for(int i=0; i < dimension_; i++)
+     trajectory_point_.q_[i] = 0.0;
+
+  trajectory_point_.time_ = 0.0;
+
+  trajectory_points_vector.push_back(trajectory_point_);
+  trajectory_points_vector.push_back(trajectory_point_);
+
+  joint_trajectory_->autocalc_timing_ = true;
+
+  ROS_INFO("Size of trajectory points vector : %d",trajectory_points_vector.size());
+  if(!joint_trajectory_->setTrajectory(trajectory_points_vector))
+    ROS_WARN("Trajectory not set correctly");
+
+  trajectory_start_time_ = robot_->hw_->current_time_;
+
+  return true;
+}
+
+void ArmTrajectoryController::setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory)
+{
+  if(joint_trajectory.size() < 1)
+  {
+    ROS_WARN("ArmTrajectoryController:: No points in trajectory");
+    return;
+  }
+
+  joint_trajectory_->setTrajectory(joint_trajectory);
+  joint_trajectory_->write("foo.txt",0.01);
+  arm_controller_lock_.lock();
+  refresh_rt_vals_ = true;
+  arm_controller_lock_.unlock();
+}
+
+void ArmTrajectoryController::getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const
+{
+}
+
+controller::JointPositionController* ArmTrajectoryController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) joint_position_controllers_.size(); i++)
+  {
+    if(joint_position_controllers_[i]->getJointName() == name)
+    {
+      return joint_position_controllers_[i];
+    }
+  }
+    return NULL;
+}
+
+int ArmTrajectoryController::getJointControllerPosByName(std::string name)
+{
+  for(int i=0; i< (int) joint_position_controllers_.size(); i++)
+  {
+    if(joint_position_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+void ArmTrajectoryController::update(void)
+{
+  double sample_time(0.0);
+  if(refresh_rt_vals_)
+  {
+    trajectory_start_time_ = robot_->hw_->current_time_;
+    refresh_rt_vals_ = false;
+  }
+  if(arm_controller_lock_.trylock())
+  {
+    sample_time = robot_->hw_->current_time_ - trajectory_start_time_;
+    joint_trajectory_->sample(trajectory_point_,sample_time);
+
+//    ROS_INFO("sample_time: %f", sample_time); 
+    for(unsigned int i=0; i < joint_cmd_rt_.size(); ++i)
+    {
+      joint_cmd_rt_[i] = trajectory_point_.q_[i];
+      cout << " " << joint_cmd_rt_[i];
+    }
+    cout << endl;
+    arm_controller_lock_.unlock();
+  }
+
+  for(unsigned int i=0;i<joint_position_controllers_.size();++i)
+    joint_position_controllers_[i]->setCommand(joint_cmd_rt_[i]);
+
+  updateJointControllers();
+}
+
+void ArmTrajectoryController::updateJointControllers(void)
+{
+  for(unsigned int i=0;i<joint_position_controllers_.size();++i)
+    joint_position_controllers_[i]->update();
+}
+
+//------ Arm controller node --------
+
+ROS_REGISTER_CONTROLLER(ArmTrajectoryControllerNode)
+
+ArmTrajectoryControllerNode::ArmTrajectoryControllerNode()
+  : Controller(), node_(ros::node::instance())
+{
+  std::cout<<"Controller node created"<<endl;
+  c_ = new ArmTrajectoryController();
+}
+
+ArmTrajectoryControllerNode::~ArmTrajectoryControllerNode()
+{
+  /* node_->unadvertise_service(service_prefix_ + "/set_command");
+  node_->unadvertise_service(service_prefix_ + "/set_command_array");
+  node_->unadvertise_service(service_prefix_ + "/get_command");
+  node_->unadvertise_service(service_prefix_ + "/set_target");
+  */
+   if(topic_name_ptr_ && topic_name_.c_str())
+  {
+    std::cout << "unsub arm controller" << topic_name_ << std::endl;
+    node_->unsubscribe(topic_name_);
+  }
+
+  delete c_;
+}
+
+void ArmTrajectoryControllerNode::update()
+{
+  c_->update();
+}
+
+bool ArmTrajectoryControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  ROS_INFO("Loading ArmTrajectoryControllerNode.");
+  service_prefix_ = config->Attribute("name");
+  ROS_INFO("The service_prefix_ is %s",service_prefix_.c_str());
+
+  if(c_->initXml(robot, config))  // Parses subcontroller configuration
+  {
+/*    node_->advertise_service(service_prefix_ + "/set_command", &ArmTrajectoryControllerNode::setJointPosHeadless, this);
+    node_->advertise_service(service_prefix_ + "/set_command_array", &ArmTrajectoryControllerNode::setJointPosSrv, this);
+    node_->advertise_service(service_prefix_ + "/get_command", &ArmTrajectoryControllerNode::getJointPosCmd, this);
+    node_->advertise_service(service_prefix_ + "/set_target", &ArmTrajectoryControllerNode::setJointPosTarget, this);
+*/
+    topic_name_ptr_ = config->FirstChildElement("listen_topic");
+    if(topic_name_ptr_)
+    {
+      topic_name_= topic_name_ptr_->Attribute("name");
+      if(!topic_name_.c_str())
+      {
+        std::cout<<" A listen _topic is present in the xml file but no name is specified\n";
+        return false;
+      }
+      node_->subscribe(topic_name_, traj_msg_, &ArmTrajectoryControllerNode::CmdTrajectoryReceived, this, 1);
+      ROS_INFO("Listening to topic: %s",topic_name_.c_str());
+    }
+    ROS_INFO("Initialized controller");
+    return true;
+  }
+  ROS_INFO("Could not initialize controller");
+  return false;
+}
+
+void ArmTrajectoryControllerNode::CmdTrajectoryReceived()
+{
+   std::vector<trajectory::Trajectory::TPoint> tp;
+
+  this->ros_lock_.lock();
+  tp.resize(traj_msg_.get_points_size());
+
+  if(traj_msg_.get_points_size() > 0)
+  {
+     if((int) traj_msg_.points[0].get_positions_size() != (int) c_->dimension_)
+        return;
+  }
+  else
+     return;
+
+  for(int i=0; i < (int) traj_msg_.get_points_size(); i++)
+  {
+     tp[i].setDimension((int) c_->dimension_);
+     for(int j=0; j < (int) c_->dimension_; j++)
+     {
+        tp[i].q_[j] = traj_msg_.points[i].positions[j];
+        tp[i].time_ = traj_msg_.points[i].time;
+     }
+  }
+
+  this->c_->setTrajectoryCmd(tp);
+  this->ros_lock_.unlock();
+}
+
+
+bool ArmTrajectoryControllerNode::getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::request &req,
+                    pr2_mechanism_controllers::GetJointPosCmd::response &resp)
+{
+  pr2_mechanism_controllers::JointPosCmd cmd;
+  c_->getJointPosCmd(cmd);
+  resp.command = cmd;
+  return true;
+}

Added: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2008-11-26 02:00:08 UTC (rev 7322)
@@ -0,0 +1,97 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <ros/node.h>
+#include <pr2_mechanism_controllers/JointTraj.h>
+
+static int done = 0;
+
+void finalize(int donecare)
+{
+  done = 1;
+}
+
+int main( int argc, char** argv )
+{
+
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("test_arm_trajectory_controller"); 
+
+  signal(SIGINT,  finalize);
+  signal(SIGQUIT, finalize);
+  signal(SIGTERM, finalize);
+
+
+  /*********** Start moving the robot ************/
+  pr2_mechanism_controllers::JointTraj cmd;
+
+  int num_points = 2;
+  int num_joints = 5;
+
+  cmd.set_points_size(num_points);
+
+  for(int i=0; i<num_points; i++)
+    cmd.points[i].set_positions_size(num_joints);
+
+  cmd.points[0].positions[0] = 0.0;
+  cmd.points[0].positions[1] = 0.0;
+  cmd.points[0].positions[2] = 0.0;
+  cmd.points[0].positions[3] = 0.0;
+  cmd.points[0].positions[4] = 0.0;
+  cmd.points[0].time = 0.0;
+
+  cmd.points[1].positions[0] = 1.0;
+  cmd.points[1].positions[1] = 1.0;
+  cmd.points[1].positions[2] = 0.2;
+  cmd.points[1].positions[3] = 0.5;
+  cmd.points[1].positions[4] = 0.4;
+  cmd.points[1].time = 10.0;
+
+  node->advertise<pr2_mechanism_controllers::JointTraj>("arm_trajectory_command",1);
+  sleep(1);
+  node->publish("arm_trajectory_command",cmd);
+  sleep(4);
+
+  ros::Time start_time = ros::Time::now();
+  ros::Duration sleep_time(0.01);
+
+/*  while(!done)
+  {
+    ros::Duration delta_time = ros::Time::now() - start_time;
+    sleep_time.sleep();
+  }
+*/
+  ros::fini();
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7448
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7448&view=rev
Author:   vijaypradeep
Date:     2008-12-02 04:15:13 +0000 (Tue, 02 Dec 2008)

Log Message:
-----------
- Adding BasePositionController. Commands in odom frame work in gazebo.
- Still need to test frame transform code for sending commands in frames other than the odom frame.
- Removed global variables (node & service_prefix) in base_controller.h, and instead made them part of the BaseControllerNode.
- Added script to command the base's position in the odometric frame

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-12-02 03:47:58 UTC (rev 7447)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-12-02 04:15:13 UTC (rev 7448)
@@ -9,6 +9,7 @@
 
 rospack_add_library( pr2_mechanism_controllers
   src/base_controller.cpp
+  src/base_position_controller.cpp
   src/laser_scanner_controller.cpp
   src/laser_scanner_traj_controller.cpp
   src/laser_scanner_velocity_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-02 03:47:58 UTC (rev 7447)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-02 04:15:13 UTC (rev 7448)
@@ -480,11 +480,6 @@
 
     misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
-  };
-
-    /** \brief A namespace ostream overload for displaying parameters */
-  std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
-
     /*
      * \brief pointer to ros node
      */
@@ -493,6 +488,11 @@
      * \brief save service name prefix for unadvertise on exit
      */
     std::string service_prefix;
+  };
+
+    /** \brief A namespace ostream overload for displaying parameters */
+  std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
+
 }
 
 

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_position_controller.h	2008-12-02 04:15:13 UTC (rev 7448)
@@ -0,0 +1,141 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#ifndef PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
+#define PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H
+
+#include "pr2_mechanism_controllers/base_controller.h"
+#include "tf/transform_listener.h"
+#include "std_msgs/PoseStamped.h"
+#include "std_msgs/Point.h"
+
+#include "misc_utils/advertised_service_guard.h"
+
+namespace pr2_mechanism_controllers
+{
+
+/**
+ * This is a simple math utility to generate velocity commands in order to close the loop around position (and rotation).
+ */
+class BasePositionControlUtil
+{
+public:
+
+  BasePositionControlUtil() ;
+  ~BasePositionControlUtil() ;
+  
+  bool initXml(TiXmlElement *config) ;
+
+  /**
+   * Determines the base's x,y,w control velocities to try to achieve the commanded position.
+   * \param commanded_pos The [x,y,w] position we're trying to acheive
+   * \param actual_pos Our current position
+   * \param elapsed_time The time between our current update and the previous update
+   */
+  tf::Vector3 updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double elapsed_time) ;
+  
+private:
+  control_toolbox::Pid pid_x_ ;                //!< Input: odom x error.   Output: Odom x velocity command
+  control_toolbox::Pid pid_y_ ;                //!< Input: odom y error.   Output: Odom y velocity command
+  control_toolbox::Pid pid_w_ ;                //!< Input: ang  w error.   Output: Odom/Base angular velocity command
+} ;
+
+
+class BasePositionControllerNode : public controller::Controller
+{
+public :
+  BasePositionControllerNode() ;
+  ~BasePositionControllerNode() ;
+
+  /**
+   * Initializes the Controller.
+   * A list of XML sections are necessary for this to work
+   * 
+   * 
+   * 
+   * 
+   */
+  bool initXml(mechanism::RobotState *robot_state, TiXmlElement *config) ;
+  
+  /**
+   * Realtime safe update method called from the realtime loop. Compares the target position to the current position,
+   * generates error terms, and then uses these to generate a Velocity command for BaseController
+   */
+  void update() ;
+
+  /**
+   * Sets the target pose of the controller. The cmd is transformed and stored into the odometric frame using the latest transform. The x,y position
+   * of the pose define the target location of the robot (in the odometric frame). The theta rotation of the robot is defined by the pure z rotation that is
+   * closest to the orientation of the pose in the odometric frame.
+   * \param cmd The pose that we want to reach
+   */
+  void setPoseCommand(std_msgs::PoseStamped cmd) ;
+
+  /**
+   * Sets an x,y,theta position for the base to reach in wheel odometry frame.  This command doesn't do any transforms. It simply sets the PID targets
+   * for the 3 different axes of the base.
+   * \param x The x position that we want to reach [in the odometric frame]
+   * \param y The y position that we want to reach [in the odometric frame]
+   * \param w The theta angle that we want to reach [in the odometric frame]
+   */
+  void setPoseOdomFrameCommand(double x, double y, double w) ;
+  
+private :
+  BasePositionControlUtil base_position_control_util_ ;        // Does the math to compute a command velocity
+  controller::BaseControllerNode base_controller_node_ ;       // Converts a commanded velocity into a wheel velocities and turret angles
+  ros::node *node_ ;
+
+  mechanism::RobotState *robot_state_ ;
+  
+  double last_time_ ;                                          // Store the last time that we called the update
+  
+  SubscriptionGuard guard_set_pose_command_ ;                  // Automatically unsubscribes set_pose_cmd
+  void setPoseCommandCallback() ;                              // Ros callback for "set_pose_command" messages
+
+  SubscriptionGuard guard_set_pose_odom_frame_command_ ;       // Automatically unsubscribes set_pose_odom_frame_cmd
+  void setPoseOdomFrameCommandCallback() ;                     // Ros callback for "set_pose_command_odom_frame" messages
+  
+  tf::Vector3 xyt_target_ ;                                    // The current x,y,theta target in position space (NOT velocity space)
+  
+  tf::TransformListener tf_ ;                                  // Transformer used to convert commands from native frame into odometric frame
+  std::string odom_frame_name_ ;                               // Stores the name of the odometric frame. This is the frame that we control in
+  
+  // Message Holders
+  std_msgs::PoseStamped pose_cmd_ ;
+  std_msgs::Point pose_odom_frame_cmd_ ;
+} ;
+
+}
+
+#endif // PR2_MECHANISM_CONTROLLERS_BASE_POSITION_CONTROLLER_H

Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py	2008-12-02 04:15:13 UTC (rev 7448)
@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Vijay Pradeep
+# (Derived from pointhead.py)
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG)
+
+import sys
+import os
+import string
+from time import sleep
+
+import rospy
+from std_msgs.msg import PointStamped, Point
+from robot_msgs.msg import JointCmd
+
+def control_base_pose_odom_frame(x,y,w):
+    head_angles = rospy.Publisher('base_position_controller/set_pose_odom_frame_command', Point)
+    rospy.init_node('base_position_commander', anonymous=True)
+    p = Point(x, y, w) ;
+    sleep(1)
+    head_angles.publish( p  )
+    sleep(1)
+
+def usage():
+    return "%s [pos_x] [pos_y] [theta (rad)]"%sys.argv[0]
+
+if __name__ == "__main__":
+
+    if len(sys.argv) ==4:
+        control_base_pose_odom_frame(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]) )
+    else:
+        print usage()
+        sys.exit(1)
+


Property changes on: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/control_base_position.py
___________________________________________________________________
Added: svn:executable
   + *

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_position_controller.cpp	2008-12-02 04:15:13 UTC (rev 7448)
@@ -0,0 +1,256 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "pr2_mechanism_controllers/base_position_controller.h"
+#include "angles/angles.h"                                              // For angular distance error calculation
+
+using namespace pr2_mechanism_controllers ;
+using namespace std ;
+
+BasePositionControlUtil::BasePositionControlUtil()
+{
+
+}
+
+
+BasePositionControlUtil::~BasePositionControlUtil()
+{
+  
+  
+}
+
+bool BasePositionControlUtil::initXml(TiXmlElement *config)
+{
+  TiXmlElement *pid_x_elem = config->FirstChildElement("pid_x") ;
+  TiXmlElement *pid_y_elem = config->FirstChildElement("pid_y") ;
+  TiXmlElement *pid_w_elem = config->FirstChildElement("pid_w") ;
+
+  if (!pid_x_elem || !pid_y_elem || !pid_w_elem)
+  {
+    printf("BasePositionController:: Error loading XML\n") ;
+    return false ;
+  }
+  
+  bool success_x = pid_x_.initXml(pid_x_elem) ;
+  bool success_y = pid_y_.initXml(pid_y_elem) ;
+  bool success_w = pid_w_.initXml(pid_w_elem) ;
+  
+  if (!success_x || !success_y || !success_w)
+  {
+    printf("BasePositionController:: Error loading pid controllers XML\n") ;
+    return false ;    
+  }
+  
+  return true ;
+}
+
+tf::Vector3 BasePositionControlUtil::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double time_elapsed)
+{
+  double err_x = actual_pos.x() - commanded_pos.x() ;
+  double err_y = actual_pos.y() - commanded_pos.y() ;
+  double err_w = angles::shortest_angular_distance(commanded_pos.z(), actual_pos.z()) ;
+  
+  tf::Vector3 velocity_cmd ;
+  
+  double odom_cmd_x = pid_x_.updatePid(err_x, time_elapsed) ;            // Translation X in the odometric frame
+  double odom_cmd_y = pid_y_.updatePid(err_y, time_elapsed) ;            // Translation Y in the odometric frame
+
+  // Rotate the translation commands so that they're in the base frame (instead of the odom frame)
+  velocity_cmd.setX( odom_cmd_x*cos(actual_pos.z()) + odom_cmd_y*sin(actual_pos.z())) ;
+  velocity_cmd.setY(-odom_cmd_x*sin(actual_pos.z()) + odom_cmd_y*cos(actual_pos.z())) ;
+
+  velocity_cmd.setZ(pid_w_.updatePid(err_w, time_elapsed)) ;             // Rotation command is same is Odom and Base frames
+  
+  return velocity_cmd ;
+}
+
+ROS_REGISTER_CONTROLLER(BasePositionControllerNode)
+
+BasePositionControllerNode::BasePositionControllerNode() : node_(ros::node::instance()), tf_(*node_)
+{
+  xyt_target_.setX(0) ;
+  xyt_target_.setY(0) ;
+  xyt_target_.setZ(0) ;
+}
+
+BasePositionControllerNode::~BasePositionControllerNode()
+{
+  
+}
+
+bool BasePositionControllerNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
+{
+  ROS_INFO("BasePositionControllerNode:: initXml()") ;
+  
+  
+  bool success ;
+  
+  last_time_ = robot_state->hw_->current_time_ ;
+  
+  string service_prefix = config->Attribute("name") ;
+  
+  robot_state_ = robot_state ;
+  
+  // Initialize the BasePositionControlUtil
+  TiXmlElement *base_pos_control_util_elem = config->FirstChildElement("base_position_control_util") ;
+  if (!base_pos_control_util_elem)
+  {
+    ROS_ERROR("Cannot start BasePositionControllerNode. Cannot find xml element BasePositionControlUtil") ;
+    return false ;
+  }
+  success = base_position_control_util_.initXml(base_pos_control_util_elem) ;  
+  if (!success)
+  {
+    ROS_ERROR("Cannot start BasePositionControllerNode. Error initializing XML for BasePositionControlUtil") ;
+    return false ;
+  }
+
+  // Initialize the BaseControllerNode
+  TiXmlElement *base_controller_node_elem = config->FirstChildElement("base_controller_node") ;
+  if (!base_controller_node_elem)
+  {
+    ROS_ERROR("Cannot start BasePositionControllerNode. Cannot find xml element base_controller_node") ;
+    return false ;
+  }
+
+  TiXmlElement *base_controller_node_controllers_elem = base_controller_node_elem->FirstChildElement("controller") ;
+  //! \todo Add a check to make sure that this XML snippet is for a BaseController
+  if (!base_controller_node_controllers_elem)
+  {
+    ROS_ERROR("Cannot start BasePositionControllerNode. Cannot find xml element 'controller' within 'base_controller_node'") ;
+    return false ;
+  }
+  
+  base_controller_node_.initXml(robot_state, base_controller_node_controllers_elem) ;
+  
+  // Initialize the BasePositionControllerNode
+  TiXmlElement *odom_frame_elem = config->FirstChildElement("odometry_frame") ;
+  const char* odom_frame = odom_frame_elem->Attribute("name") ;
+  if (odom_frame == NULL)
+  {
+    ROS_ERROR("Cannot start BasePositionControllerNode. odometry_frame element not defined with attribute name") ;
+    return false ;    
+  }
+  odom_frame_name_ = odom_frame ;
+  
+  node_->subscribe(service_prefix + "/set_pose_command", pose_cmd_, &BasePositionControllerNode::setPoseCommandCallback, this, 1) ;
+  guard_set_pose_command_.set(service_prefix + "/set_pose_command") ;
+
+  node_->subscribe(service_prefix + "/set_pose_odom_frame_command", pose_odom_frame_cmd_, &BasePositionControllerNode::setPoseOdomFrameCommandCallback, this, 1) ;
+  guard_set_pose_odom_frame_command_.set(service_prefix + "/set_pose_odom_frame_command") ;
+  
+  return true ;
+}
+
+void BasePositionControllerNode::update()
+{
+  // Grab the current odometric position
+  double x, y, w, vx, vy, vw ;
+  base_controller_node_.getOdometry(x, y, w, vx, vy, vw) ;
+  tf::Vector3 xyt_current(x, y, w) ;
+  
+  // Determine Elapsed Time
+  double cur_time = robot_state_->hw_->current_time_ ;
+  double time_elapsed = cur_time - last_time_ ;
+  last_time_ = cur_time ;
+  
+  // Determine next velocity to command
+  tf::Vector3 vel_cmd ;
+  vel_cmd = base_position_control_util_.updateControl(xyt_target_, xyt_current, time_elapsed) ;
+  
+  // Command the velocity
+  base_controller_node_.setCommand(vel_cmd.x(), vel_cmd.y(), vel_cmd.z()) ;
+  
+  base_controller_node_.update() ;
+  
+  //static unsigned int count = 0 ;
+  //count++ ;
+  //if (count % 500 == 0)
+  //{
+  //  ROS_INFO("BasePositionControllerNode::Velocity Command: %f %f %f", vel_cmd.x(), vel_cmd.y(), vel_cmd.z()) ;
+  //  ROS_INFO("                            Current Position: %f %f %f", x, y, w) ;
+  //  ROS_INFO("                            Command Position: %f %f %f", xyt_target_.x(), xyt_target_.y(), xyt_target_.w()) ;
+  //}
+}
+
+void BasePositionControllerNode::setPoseCommandCallback()
+{
+  setPoseCommand(pose_cmd_) ;
+}
+
+void BasePositionControllerNode::setPoseOdomFrameCommandCallback()
+{
+  setPoseOdomFrameCommand(pose_odom_frame_cmd_.x, pose_odom_frame_cmd_.y, pose_odom_frame_cmd_.z) ;
+}
+
+//! \todo This method has not yet been tested
+void BasePositionControllerNode::setPoseCommand(std_msgs::PoseStamped cmd)
+{
+  std_msgs::PoseStamped pose_odom ;               // Stores the pose in the odometric frame
+  cmd.header.stamp = ros::Time(0.0) ;             // Transform using the latest transform
+  tf_.transformPose(odom_frame_name_, cmd, pose_odom) ;
+
+  tf::Quaternion orientation_odom ;               // Orientation in the odometric frame
+  tf::QuaternionMsgToTF(pose_odom.pose.orientation, orientation_odom) ;
+
+  // For convenience, use 1-Letter names for quaternion terms
+  const double& w = orientation_odom.w() ;
+  const double& x = orientation_odom.x() ;
+  const double& y = orientation_odom.y() ;
+  const double& z = orientation_odom.z() ;
+
+  // Compute the Z Rotation from the quaternion
+  // Taken from Section 7.4: http://ai.stanford.edu/~diebel/attitude/attitude.pdf
+  //! \todo Use a more stable way to compute the z rotation
+  double z_rot ;
+  if (w > .99999)                       // Calculation becomes unstable near 0 rotation
+    z_rot = 2*z ;
+  else
+    z_rot = 2*acos(w)*z / sqrt(x*x+y*y+z*z) ;
+  
+  setPoseOdomFrameCommand(pose_odom.pose.position.x,
+                          pose_odom.pose.position.y,
+                          0.0) ;
+                          //z_rot) ;                    // Ignoring the yaw commands for now
+}
+
+void BasePositionControllerNode::setPoseOdomFrameCommand(double x, double y, double w)
+{
+  ROS_INFO("BasePositionControllerNode:: Odom Frame Position Command: %f %f %f\n", x, y, w) ;
+  
+  //! \todo Mutex this data type
+  xyt_target_.setX(x) ;
+  xyt_target_.setY(y) ;
+  xyt_target_.setZ(w) ;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7829
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7829&view=rev
Author:   sachinchitta
Date:     2008-12-09 21:36:43 +0000 (Tue, 09 Dec 2008)

Log Message:
-----------
velocity limits are now imposed on the magnitude of translational velocity

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-09 21:34:05 UTC (rev 7828)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-09 21:36:43 UTC (rev 7829)
@@ -388,6 +388,8 @@
 
     double odometer_angle_;
 
+    double max_trans_vel_magnitude_;
+
     friend class BaseControllerNode;
   };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-09 21:34:05 UTC (rev 7828)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-09 21:36:43 UTC (rev 7829)
@@ -52,8 +52,6 @@
 
 ROS_REGISTER_CONTROLLER(BaseController)
 
-
-
 BaseController::BaseController() : num_wheels_(0), num_casters_(0)
 {
    MAX_DT_ = 0.01;
@@ -106,8 +104,22 @@
 {
 
   pthread_mutex_lock(&base_controller_lock_);
-  cmd_vel_t_.x = filters::clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
-  cmd_vel_t_.y = filters::clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+
+  double vel_mag = sqrt(cmd_vel.x*cmd_vel.x+cmd_vel.y*cmd_vel.y);
+  double clamped_vel_mag = filters::clamp(vel_mag,-max_trans_vel_magnitude_,max_trans_vel_magnitude_);
+
+  if(vel_mag > EPS)
+  {
+    cmd_vel_t_.x = cmd_vel.x * clamped_vel_mag/vel_mag;
+    cmd_vel_t_.y = cmd_vel.y * clamped_vel_mag/vel_mag;
+  }
+  else
+  {
+    cmd_vel_t_.x = 0.0;
+    cmd_vel_t_.y = 0.0;
+  }
+//  cmd_vel_t_.x = filters::clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+//  cmd_vel_t_.y = filters::clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
   cmd_vel_t_.z = filters::clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
 #if 0
@@ -345,7 +357,8 @@
     elt = config->NextSiblingElement("map");
   }
 
-
+  max_trans_vel_magnitude_  = fabs(max_vel_.x);
+ 
   ROS_INFO("BaseController:: kp_speed %f",kp_speed_);
   ROS_INFO("BaseController:: kp_caster_steer  %f",caster_steer_vel_gain_);
   ROS_INFO("BaseController:: timeout %f",timeout_);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7880
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7880&view=rev
Author:   sachinchitta
Date:     2008-12-10 02:50:37 +0000 (Wed, 10 Dec 2008)

Log Message:
-----------
base controller will now transmit a covariance message under the topic name /base_controller/odometry_covariance

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-10 02:47:04 UTC (rev 7879)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-10 02:50:37 UTC (rev 7880)
@@ -56,6 +56,7 @@
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/BaseVel.h>
 #include <pr2_msgs/Odometer.h>
+#include <pr2_msgs/Covariance2D.h>
 
 #include <misc_utils/realtime_publisher.h>
 
@@ -258,6 +259,10 @@
 
     double wheel_radius_multiplier_rear_;
 
+//    NEWMAT::Matrix odometry_residual_;
+
+    double odometry_residual_max_;
+
     private:
 
     bool new_cmd_available_; /** true when new command received by node */
@@ -481,6 +486,8 @@
 
     misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
+    misc_utils::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+
     /*
      * \brief pointer to ros node
      */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-10 02:47:04 UTC (rev 7879)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-10 02:50:37 UTC (rev 7880)
@@ -41,8 +41,8 @@
 #define EPS 1e-5
 #define CMD_VEL_TRANS_EPS 1e-5
 #define CMD_VEL_ROT_EPS 1e-5
+static const double MIN_BASE_CONTROLLER_COVARIANCE = 0.000001;
 
-
 using namespace ros;
 using namespace std;
 using namespace controller;
@@ -610,6 +610,8 @@
   odometer_angle_ += base_odom_delta.z;
 }
 
+
+
 void BaseController::computeBaseVelocity()
 {
   Matrix A(2*num_wheels_,1);
@@ -620,6 +622,8 @@
   libTF::Vector caster_2d_velocity;
   libTF::Vector wheel_caster_steer_component;
 
+  NEWMAT::Matrix odometry_residual(2*num_wheels_,1);
+
   caster_2d_velocity.x = 0;
   caster_2d_velocity.y = 0;
 
@@ -649,6 +653,12 @@
   }
   //   D = pseudoInverse(C)*A;
   D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
+
+  odometry_residual = C*D-A;
+  odometry_residual_max_ = odometry_residual.MaximumAbsoluteValue();
+
+//  cout << "Residual " << endl << odometry_residual << endl;
+
   base_odom_velocity_.x = (double)D.element(0,0);
   base_odom_velocity_.y = (double)D.element(1,0);
   base_odom_velocity_.z = (double)D.element(2,0);
@@ -776,6 +786,7 @@
   publisher_ = NULL;
   transform_publisher_ = NULL;
   odometer_publisher_ = NULL;
+  covariance_publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
@@ -792,6 +803,7 @@
   delete publisher_;
   delete transform_publisher_;
   delete odometer_publisher_;
+  delete covariance_publisher_;
   delete c_;
 }
 
@@ -819,6 +831,36 @@
       last_time_message_sent_ = time;
     }
 
+    if (covariance_publisher_->trylock())
+    {
+      double base_odom_velocity_mag = sqrt(c_->base_odom_velocity_.x*c_->base_odom_velocity_.x+c_->base_odom_velocity_.y*c_->base_odom_velocity_.y+c_->base_odom_velocity_.z*c_->base_odom_velocity_.z);
+      double dirn_x(0),dirn_y(0),dirn_z(0);
+
+      if (base_odom_velocity_mag > EPS)
+      {
+        dirn_x = fabs(c_->base_odom_velocity_.x/base_odom_velocity_mag);
+        dirn_y = fabs(c_->base_odom_velocity_.y/base_odom_velocity_mag);   
+        dirn_z = fabs(c_->base_odom_velocity_.z/base_odom_velocity_mag);
+      }
+      else
+      {
+        dirn_x = 0.0;
+        dirn_y = 0.0;
+        dirn_z = 0.0;
+      }
+      covariance_publisher_->msg_.Cxx = c_->odometry_residual_max_;
+
+      covariance_publisher_->msg_.Cxx = std::max<double>(c_->odometry_residual_max_*dirn_x,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cyy = std::max<double>(c_->odometry_residual_max_*dirn_y,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Czz = std::max<double>(c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+
+      covariance_publisher_->msg_.Cxy = std::max<double>(c_->odometry_residual_max_*dirn_x+c_->odometry_residual_max_*dirn_y,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cxz = std::max<double>(c_->odometry_residual_max_*dirn_x+c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cyz = std::max<double>(c_->odometry_residual_max_*dirn_y+c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+
+      covariance_publisher_->unlockAndPublish() ;
+    }
+
     if (transform_publisher_->trylock())
     {
       double x=0,y=0,yaw=0,vx,vy,vyaw;
@@ -951,7 +993,11 @@
     delete transform_publisher_ ;
   transform_publisher_ = new misc_utils::RealtimePublisher <tf::TransformArray> ("TransformArray", 5) ;
 
+  if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete covariance_publisher_ ;
+  covariance_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
 
+
   node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
 
   double multiplier;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7925
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7925&view=rev
Author:   sachinchitta
Date:     2008-12-10 22:07:39 +0000 (Wed, 10 Dec 2008)

Log Message:
-----------
new version of the base controller

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-10 22:07:39 UTC (rev 7925)
@@ -0,0 +1,515 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+#include <robot_mechanism_controllers/joint_pd_controller.h>
+
+// Services
+#include <pr2_mechanism_controllers/SetBaseCommand.h>
+#include <pr2_mechanism_controllers/GetBaseCommand.h>
+#include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
+
+#include <libTF/Pose3D.h>
+#include <urdf/URDF.h>
+
+#include <newmat10/newmat.h>
+#include <newmat10/newmatio.h>
+#include <newmat10/newmatap.h>
+
+#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/BaseVel.h>
+#include <pr2_msgs/Odometer.h>
+
+#include <misc_utils/realtime_publisher.h>
+
+#include <tf/TransformArray.h>
+
+#include <pthread.h>
+#include <trajectory/trajectory.h>
+
+namespace controller
+{
+  #define KP_SPEED_DEFAULT 100
+  #define DEFAULT_WHEEL_RADIUS 0.079
+
+  /*! \class
+    \brief This class holds local information for the links in the base (wheels and casters). It includes position, name,
+    controller, pointer to the corresponding joint in the Robot structure, pointer to parent and a local ID.
+  */
+   class BaseParam
+  {
+    public:
+
+    friend std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
+
+    BaseParam():direction_multiplier_(1),name_(" "),joint_state_(NULL),parent_(NULL){};
+
+    ~BaseParam(){}
+
+    libTF::Vector pos_; /** position of the link*/
+
+    std::string name_; /** name of joint corresponding to the link */
+
+    JointVelocityController velocity_controller_; /** controller for the link */
+
+    JointPositionController position_controller_; /** position controller for the link */
+
+    mechanism::JointState *joint_state_; /** pointer to joint in Robot structure corresponding to link */
+
+    BaseParam *parent_; /** pointer to parent corresponding to link */
+
+    int local_id_; /** local id number */
+
+    int direction_multiplier_;
+  };
+
+  /*! \class
+    \brief This class inherits from Controller and implements the actual controls.
+  */
+  class BaseControllerPos : public Controller
+  {
+    public:
+
+    /*!
+     * \brief Default Constructor of the JointController class.
+     *
+     */
+    BaseControllerPos();
+
+
+    /*!
+     * \brief Destructor of the JointController class.
+     */
+    ~BaseControllerPos();
+
+
+    /*!
+     * \brief Functional way to initialize limits and gains.
+     *
+     */
+    void init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot);
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+    /*!
+     * \brief Specify the commanded velocity for the controller using a libTF::Vector data structure
+     *
+     * \param libTF::Vector cmd_vel with cmd_vel.x and cmd_vel.y specifying the forward and sideways speeds respectively while cmd_vel.z specifies the rotational speed.
+     */
+    void setCommand(libTF::Vector cmd_vel);
+
+
+    /*!
+     * \brief Get the current command. This is the command that is currently being executed by the controller and may differ from the actual command issued because of the imposition of acceleration constraints.
+     */
+    libTF::Vector getCommand();
+
+
+    /*!
+     * \brief (a) Updates commands to caster and wheels.
+     *         
+     *  Called every timestep in realtime
+     */
+    virtual void update();
+
+
+    /*!
+     * \brief struct to represent caster parameters locally.
+     */
+//    std::vector<BaseParam> base_casters_;
+    BaseParam base_casters_[4];
+
+    /*!
+     * \brief struct to represent wheel parameters locally.
+     */
+//    std::vector<BaseParam> base_wheels_;
+    BaseParam base_wheels_[8];
+
+    /*!
+     * \brief mutex lock for setting and getting commands
+     */
+    pthread_mutex_t base_controller_lock_;
+
+    /*!
+     * \brief URDF representation of the robot model
+     */
+    robot_desc::URDF urdf_model_;
+
+
+    /*!
+     * \brief returns odometry data
+     * \param x position estimate from odometry (origin is at the starting point when controller was started)
+     * \param y position estimate from odometry (origin is at the starting point when controller was started)
+     * \param w position estimate from odometry (origin is at the starting point when controller was started)
+     * \param vx velocity estimate from odometry (in the coordinate frame of the base)
+     * \param vy velocity estimate from odometry (in the coordinate frame of the base)
+     * \param vz velocity estimate from odometry (in the coordinate frame of the base)
+     */
+    void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
+
+    /*!
+     * \brief Fill a structure of the form std_msgs::RobotBase2DOdom with odometry data
+     */
+    void setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_);
+
+    /*!
+     * \brief compute the desired wheel speeds
+     */
+    void computeDesiredWheelSpeeds();
+
+    /*!
+     * \brief set the desired wheel speeds in the joint speed controllers
+     */
+    void setDesiredWheelSpeeds();
+
+    /*!
+     * \brief compute the desired caster steer
+     */
+    void computeDesiredCasterSteer(double current_sample_time, double dT);
+
+    /*!
+     * \brief set the desired caster steer
+     */
+    void setDesiredCasterSteer();
+
+    /*!
+     * \brief compute the joint commands 
+     */
+    void computeJointCommands(double current_sample_time, double dT);
+
+    /*!
+     * \brief set the joint commands
+     */
+    void setJointCommands();
+
+    /*!
+     * \brief figure out the command for the next time step by interpolating from start to end while staying within rate limits
+     * specified by max_rate
+     * \param dT - timestep
+     */
+    libTF::Vector interpolateCommand(double time);
+
+    /*!
+     * \brief iterative least squares implementation to compute odometry. This implementation computes and returns x where A*x = b
+     * \param A - newmat matrix of size m x n
+     * \param b - newmat matrix of size m x 1
+     * \param weight_type - type of weighting used in the iterations, possible choices include BubeLagan, L1norm, fair, Cauchy, Gaussian
+     * \param max_iter - maximum number of iterations
+     * \return x - the n x 1 result as a NEWMAT matrix
+     */
+    NEWMAT::Matrix iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter);
+
+    /*!
+     * \brief returns the weight matrix given the weight type and residual
+     * \param residual (a m x 1 NEWMAT matrix)
+     * \param weight_type - std::string specification of weight type
+     */
+    NEWMAT::Matrix findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type);
+
+    int ils_max_iterations_; /** maximum number of iterations for Iterative Least Squares */
+
+    std::string ils_weight_type_; /** std::string specification of weight type used for IRLS*/
+
+    /*!
+     * \brief Robot representation
+     */
+    mechanism::RobotState* robot_state_;
+
+    double wheel_radius_; /** radius of the wheel (filled in from urdf robot model) */
+
+    double wheel_radius_multiplier_front_;
+
+    double wheel_radius_multiplier_rear_;
+
+    trajectory::Trajectory *cmd_vel_trajectory_;
+
+    trajectory::Trajectory::TPoint current_vel_point_;
+
+    std::vector<trajectory::Trajectory::TPoint> cmd_vel_points_;
+
+    libTF::Vector cmd_vel_direction_;
+
+    double cmd_vel_magnitude_;
+
+    double sample_time_;
+
+    void setVelocityCmdTrajectory(libTF::Vector new_cmd, libTF::Vector max_rate);
+
+    private:
+
+    bool new_cmd_available_; /** true when new command received by node */
+
+    /*!
+     * \brief number of wheels
+     */
+    int num_wheels_;
+
+
+    /*!
+     * \brief number of casters
+     */
+    int num_casters_;
+
+    /*!
+     * \brief local gain used for speed control of the caster (to achieve resultant position control)
+     */
+    double kp_speed_;
+
+    /*!
+     * \brief compute 2D velocity of a point on a rigid body given a 2D input velocity
+     * \param pos - Vector (see libTF for more information)
+     * \param vel - Vector, vel.x and vel.y represent translational velocities in X and Y directions, vel.z represents rotational(angular velocity)
+     * \return point 2D velocity with .z component set to zero.
+     */
+    libTF::Vector computePointVelocity2D(const libTF::Vector& pos, const  libTF::Vector& vel);
+
+    /*!
+     * \brief update the individual joint controllers
+     */
+    void updateJointControllers();
+
+
+    /*!
+     * \brief speed command vector used internally to represent the current commanded speed
+     */
+    libTF::Vector cmd_vel_;
+
+
+    /*!
+     * \brief Input speed command vector represents the desired speed requested by the node. Note that this may differ from the 
+     * current commanded speed due to acceleration limits imposed by the controller. This 
+     */
+    libTF::Vector cmd_vel_t_;
+
+    /*!
+     * \brief Input speed command vector represents the desired speed requested by the node. 
+     */
+    libTF::Vector desired_vel_;
+
+    /*!
+     * \brief Position of the robot computed by odometry.
+     */
+    libTF::Vector base_odom_position_;
+
+
+    /*!
+     * \brief Speed of the robot computed by odometry.
+     */
+    libTF::Vector base_odom_velocity_;
+
+
+    std::vector<double> steer_velocity_desired_; /** vector of desired caster steer speeds */
+
+    std::vector<double> steer_position_desired_; /** vector of desired caster steer positions */
+
+    std::vector<double> steer_torque_desired_; /** vector of desired caster steer positions */
+
+    std::vector<double> wheel_speed_cmd_; /** vector of desired wheel speeds */
+
+    /*!
+     * \brief compute the speed of the base for odometry calculations
+     */
+    void computeBaseVelocity();
+
+    /*!
+     * \brief compute the odometry
+     */
+    void computeOdometry(double);
+
+
+    /*!
+     * \brief pseudo-inverse computation for NEWMAT
+     */
+    NEWMAT::Matrix pseudoInverse(const NEWMAT::Matrix M);
+
+
+    /*!
+     * \brief compute the wheel positions and set them in base_wheels_position_
+     */
+    void computeWheelPositions();
+
+
+    std::vector<libTF::Vector> base_wheels_position_; /** vector of current wheel positions */
+
+
+    /*!
+     * \brief get the joint positions and speeds and set them in steer_angle_actual_ and wheel_speed_actual_
+     */
+    void getJointValues();
+
+    /*!
+     * \brief function to add parameter to map so it can be initialized easily from the xml file
+     */
+    void addParamToMap(std::string key, double *value); 
+
+    std::vector<double> steer_angle_actual_; /** vector of actual caster steer angles */
+
+    std::vector<double> steer_angle_stored_; /** vector of stored caster steer angles */
+
+    std::vector<double> wheel_speed_actual_; /** vector of actual wheel speeds */
+
+    double last_time_; /** time corresponding to when update was last called */
+
+    double caster_steer_vel_gain_; /** gain specifying the amount of help given by the wheels to the caster steer degree of freedom */
+
+    double cmd_received_timestamp_; /** timestamp corresponding to when the command received by the node */
+
+    double timeout_; /** timeout specifying time that the controller waits before setting the current velocity command to zero */
+
+    libTF::Vector max_vel_; /** velocity limits specified externally */
+
+    libTF::Vector max_accel_; /** acceleration limits specified externally */
+
+    double MAX_DT_; /** maximum dT used in computation of interpolated velocity command */
+
+    std::map<std::string, double*> param_map_; /*< map from pointers to the params to string names */
+
+    double odometer_distance_;
+
+    double odometer_angle_;
+
+    friend class BaseControllerPosNode;
+  };
+
+  /*! \class
+    \brief This class inherits from Controller and is the ROS Node corresponding to the controller. This is the class that should be instantiated whenever the user wants to spawn a new base controller. 
+  */
+  class BaseControllerPosNode : public Controller
+  {
+    public:
+    /*!
+     * \brief Default Constructor
+     *
+     */
+    BaseControllerPosNode();
+
+    /*!
+     * \brief Destructor
+     */
+    ~BaseControllerPosNode();
+
+    /*!
+     * realtime safe update call
+     */
+    void update();
+
+    /*!
+     * \brief initialize the node from a xml config specification
+     */
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * \brief get back the odometry values from the controller itself
+     * \param x position computed by odometry 
+     * \param y position computed by odometry
+     * \param theta (yaw) computed by odometry
+     * \param x velocity computed by odometry (in local frame)
+     * \param y velocity computed by odometry (in local frame)
+     * \param theta velocity computed by odometry (in local frame)
+     */
+    void getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw);
+
+    // Services
+    bool setCommand(pr2_mechanism_controllers::SetBaseCommand::request &req,
+                    pr2_mechanism_controllers::SetBaseCommand::response &resp);
+
+    bool getCommand(pr2_mechanism_controllers::GetBaseCommand::request &req,
+                    pr2_mechanism_controllers::GetBaseCommand::response &resp);
+
+    bool getWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+                                                      pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+
+    bool setWheelRadiusMultiplier(pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+                                                      pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp);
+
+    /*
+     * \brief callback function for setting the desired velocity using a topic 
+     */
+    void setCommand(double vx, double vy, double vw);
+
+    BaseControllerPos *c_;
+
+    private:
+
+    /*!
+     * \brief deal with cmd_vel command from 2dnav stack
+     */
+    void CmdBaseVelReceived();
+    std_msgs::BaseVel baseVelMsg;
+
+    /*!
+     * \brief mutex lock for setting and getting ros messages
+     */
+    ros::thread::mutex ros_lock_;
+
+    /*!
+     * \brief std_msgs representation of an odometry message
+     */
+    std_msgs::RobotBase2DOdom odom_msg_;
+
+    double last_time_message_sent_ ;/** last time odometry message was published */
+
+    double odom_publish_delta_t_; /** time after which odometry message will be published */
+
+    double odom_publish_rate_; /** rate at which odometry message will be published ( = 1/odom_publish_delta_t_)*/
+           
+    misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the odometry msg from the update() realtime loop
+
+    misc_utils::RealtimePublisher <tf::TransformArray>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+
+    misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+
+  };
+
+    /** \brief A namespace ostream overload for displaying parameters */
+//  std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
+
+    /*
+     * \brief pointer to ros node
+     */
+    ros::node *node;
+    /*
+     * \brief save service name prefix for unadvertise on exit
+     */
+    std::string service_prefix;
+}
+
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-10 22:07:39 UTC (rev 7925)
@@ -0,0 +1,1046 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <pr2_mechanism_controllers/base_controller_pos.h>
+#include <control_toolbox/filters.h>
+#include <angles/angles.h>
+#include "ros/node.h"
+
+#define NUM_TRANSFORMS 2
+#define EPS 1e-5
+#define CMD_VEL_TRANS_EPS 1e-5
+#define CMD_VEL_ROT_EPS 1e-5
+
+using namespace ros;
+using namespace std;
+using namespace controller;
+using namespace control_toolbox;
+using namespace libTF;
+using namespace NEWMAT;
+
+ROS_REGISTER_CONTROLLER(BaseControllerPos)
+
+BaseControllerPos::BaseControllerPos() : num_wheels_(0), num_casters_(0)
+{
+   MAX_DT_ = 0.01;
+
+   max_accel_.x = 0.1;
+   max_accel_.y = 0.1;
+   max_accel_.z = 0.1;
+
+   cmd_vel_.x = 0;
+   cmd_vel_.y = 0;
+   cmd_vel_.z = 0;
+
+   desired_vel_.x = 0;
+   desired_vel_.y = 0;
+   desired_vel_.z = 0;
+
+   cmd_vel_t_.x = 0;
+   cmd_vel_t_.y = 0;
+   cmd_vel_t_.z = 0;
+
+   cmd_vel_direction_.x = 1.0;
+   cmd_vel_direction_.y = 0.0;
+   cmd_vel_direction_.z = 0.0;
+   cmd_vel_magnitude_ = 0.0;
+
+   cmd_vel_trajectory_ = new trajectory::Trajectory(4);
+
+   cmd_vel_points_.resize(2);
+   for(int i=0; i< 2; i++)
+   {
+      cmd_vel_points_[i].setDimension(4);
+      cmd_vel_points_[i].q_[0] = 1.0;
+      cmd_vel_points_[i].time_ = 0.0;
+   }
+   cmd_vel_trajectory_->setTrajectory(cmd_vel_points_);
+   current_vel_point_.setDimension(4);
+
+   kp_speed_ = KP_SPEED_DEFAULT;
+
+   base_odom_position_.x = 0;
+   base_odom_position_.y = 0;
+   base_odom_position_.z = 0;
+
+   base_odom_velocity_.x = 0;
+   base_odom_velocity_.y = 0;
+   base_odom_velocity_.z = 0;
+
+   ils_weight_type_ = "Gaussian";
+   ils_max_iterations_ = 3;
+   caster_steer_vel_gain_ = 0;
+   timeout_ = 0.1;
+
+   odometer_distance_ = 0;
+   odometer_angle_ = 0;
+
+   new_cmd_available_ = false;
+
+   pthread_mutex_init(&base_controller_lock_,NULL);
+}
+
+BaseControllerPos::~BaseControllerPos()
+{
+  delete cmd_vel_trajectory_;
+}
+
+// Set the base velocity command
+void BaseControllerPos::setCommand(libTF::Vector cmd_vel)
+{
+  pthread_mutex_lock(&base_controller_lock_);
+  cmd_vel_t_.x = filters::clamp(cmd_vel.x,-max_vel_.x, max_vel_.x);
+  cmd_vel_t_.y = filters::clamp(cmd_vel.y,-max_vel_.y, max_vel_.y);
+  cmd_vel_t_.z = filters::clamp(cmd_vel.z,-max_vel_.z, max_vel_.z);
+  cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+#if 0
+  ROS_INFO("BaseControllerPos:: command received: %f %f %f",cmd_vel.x,cmd_vel.y,cmd_vel.z);
+  ROS_INFO("BaseControllerPos:: command current: %f %f %f", cmd_vel_.x,cmd_vel_.y,cmd_vel_.z);
+
+  ROS_INFO("Base Odometry: Velocity ", base_odom_velocity_.x,base_odom_velocity_.y,base_odom_velocity_.z);
+  ROS_INFO("Base Odometry: Position ", base_odom_position_.x,base_odom_position_.y,base_odom_position_.z);
+
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+    ROS_INFO("BaseControllerPos:: wheel speed cmd:: %d %f",i,(base_wheels_[i].direction_multiplier_*wheel_speed_cmd_[i]));
+  }
+#endif
+  new_cmd_available_ = true;
+  pthread_mutex_unlock(&base_controller_lock_);
+}
+
+libTF::Vector BaseControllerPos::getCommand()// Return the current position command
+{
+  libTF::Vector cmd_vel;
+  pthread_mutex_lock(&base_controller_lock_);
+  cmd_vel.x = cmd_vel_.x;
+  cmd_vel.y = cmd_vel_.y;
+  cmd_vel.z = cmd_vel_.z;
+  pthread_mutex_unlock(&base_controller_lock_);
+  return cmd_vel;
+}
+
+void BaseControllerPos::init(std::vector<JointControlParam> jcp, mechanism::RobotState *robot_state)
+{
+   std::vector<JointControlParam>::iterator jcp_iter;
+   robot_desc::URDF::Link *link;
+   std::string joint_name;
+
+   std::string xml_content;
+   (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+
+   robot_state_ = robot_state;
+
+   // wait for robotdesc/pr2 on param server
+   while(!urdf_model_.loadString(xml_content.c_str()))
+   {
+      ROS_INFO("WARNING: base controller is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar.");
+      (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+      usleep(100000);
+   }
+
+//   base_casters_.resize(4);
+//   base_wheels_.resize(8);
+
+   for(jcp_iter = jcp.begin(); jcp_iter != jcp.end(); jcp_iter++)
+   {
+      joint_name = jcp_iter->joint_name;
+      link = urdf_model_.getJointLink(joint_name);
+
+      if(joint_name.find("rotation") != string::npos)
+      {
+         base_casters_[num_casters_].pos_.x = link->xyz[0];
+
+         base_casters_[num_casters_].pos_.y = link->xyz[1];
+         base_casters_[num_casters_].pos_.z = link->xyz[2];
+         base_casters_[num_casters_].parent_ = NULL;
+         base_casters_[num_casters_].joint_state_ = robot_state_->getJointState(joint_name);
+         base_casters_[num_casters_].local_id_ = num_casters_;
+         steer_angle_actual_.push_back(0);
+         steer_angle_stored_.push_back(0);
+         steer_position_desired_.push_back(0);
+         steer_velocity_desired_.push_back(0);
+         steer_torque_desired_.push_back(0);
+
+         base_casters_[num_casters_].position_controller_.init(robot_state_, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+         base_casters_[num_casters_].velocity_controller_.init(robot_state_, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+
+         base_casters_[num_casters_].name_ = std::string(" ");
+         base_casters_[num_casters_].name_ = std::string(joint_name);
+
+         num_casters_++;
+      }
+      if(joint_name.find("wheel") != string::npos)
+      {
+         base_wheels_[num_wheels_].pos_.x = link->xyz[0];
+         base_wheels_[num_wheels_].pos_.y = link->xyz[1];
+         base_wheels_[num_wheels_].pos_.z = link->xyz[2];
+         base_wheels_[num_wheels_].name_ = joint_name;
+         base_wheels_[num_wheels_].parent_ = NULL;
+         base_wheels_[num_wheels_].joint_state_ = robot_state_->getJointState(joint_name);
+         base_wheels_[num_wheels_].local_id_ = num_wheels_;
+         if(joint_name.find("_r_") != string::npos)
+            base_wheels_[num_wheels_].direction_multiplier_ = 1;
+         else
+            base_wheels_[num_wheels_].direction_multiplier_ = 1;
+
+         wheel_speed_actual_.push_back(0);
+         libTF::Vector *v=new libTF::Vector();
+         base_wheels_position_.push_back(*v);
+         wheel_speed_cmd_.push_back(0);
+
+         base_wheels_[num_wheels_].velocity_controller_.init(robot_state_, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+         base_wheels_[num_wheels_].position_controller_.init(robot_state_, joint_name, Pid(jcp_iter->p_gain,jcp_iter->i_gain,jcp_iter->d_gain,jcp_iter->windup));
+
+         num_wheels_++;
+         delete v;
+      }
+   }
+
+   for(int i =0; i < num_wheels_; i++)
+   {
+      link = urdf_model_.getJointLink(base_wheels_[i].name_);
+      std::string parent_name = link->parent->joint->name;
+      for(int j =0; j < num_casters_; j++)
+      {
+         if(parent_name == base_casters_[j].name_)
+         {
+            base_wheels_[i].parent_ = &base_casters_[j];
+            break;
+         }
+      }
+   }
+
+   if(num_wheels_ > 0)
+   {
+      link = urdf_model_.getJointLink(base_wheels_[0].name_);
+      robot_desc::URDF::Link::Geometry::Cylinder *wheel_geom = dynamic_cast<robot_desc::URDF::Link::Geometry::Cylinder*> (link->collision->geometry->shape);
+      if (wheel_geom)
+         wheel_radius_ = wheel_geom->radius;
+      else
+      {
+         ROS_WARN("Wheel geom is not found in URDF, base controller will set wheel_radius_ for %s to DEFAULT_WHEEL_RADIUS %f\n.",base_wheels_[0].name_.c_str(),DEFAULT_WHEEL_RADIUS);
+         wheel_radius_ = DEFAULT_WHEEL_RADIUS;
+      }
+   }
+   else
+   {
+      wheel_radius_ = DEFAULT_WHEEL_RADIUS;
+   }
+
+   last_time_ = robot_state_->hw_->current_time_;
+
+   sample_time_ = robot_state_->hw_->current_time_;
+
+   cmd_received_timestamp_ = robot_state_->hw_->current_time_;
+}
+
+bool BaseControllerPos::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
+{
+
+  ROS_INFO("BaseControllerPos:: name: %s",config->Attribute("name")); 
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  std::vector<JointControlParam> jcp_vec;
+  JointControlParam jcp;
+  while (elt){
+    TiXmlElement *jnt = elt->FirstChildElement("joint");
+    jcp.p_gain = atof(jnt->FirstChildElement("pid")->Attribute("p"));
+    jcp.i_gain = atof(jnt->FirstChildElement("pid")->Attribute("i"));
+    jcp.d_gain = atof(jnt->FirstChildElement("pid")->Attribute("d"));
+    jcp.windup = atof(jnt->FirstChildElement("pid")->Attribute("iClamp"));
+    jcp.control_type = (std::string) elt->Attribute("type");
+    jcp.joint_name = jnt->Attribute("name");
+    jcp_vec.push_back(jcp);
+
+    ROS_INFO("BaseControllerPos:: joint name: %s",jcp.joint_name.c_str()); 
+    ROS_INFO("BaseControllerPos:: controller type: %s\n",jcp.control_type.c_str()); 
+
+    elt = elt->NextSiblingElement("controller");
+  }
+
+  addParamToMap("kp_speed",&kp_speed_);
+  addParamToMap("kp_caster_steer",&caster_steer_vel_gain_);
+  addParamToMap("timeout",&timeout_);
+  addParamToMap("max_x_dot",&(max_vel_.x));
+  addParamToMap("max_y_dot",&(max_vel_.y));
+  addParamToMap("max_yaw_dot",&(max_vel_.z));
+  addParamToMap("max_x_accel",&(max_accel_.x));
+  addParamToMap("max_y_accel",&(max_accel_.y));
+  addParamToMap("max_yaw_accel",&(max_accel_.z));
+
+  elt = config->FirstChildElement("map");
+
+  while(elt)
+  {
+    if(elt->Attribute("name") == std::string("velocity_control"))
+    {
+      TiXmlElement *elt_key = elt->FirstChildElement("elem");
+      while(elt_key)
+      {
+        *(param_map_[elt_key->Attribute("key")]) = atof(elt_key->GetText());
+        elt_key = elt_key->NextSiblingElement("elem");
+      }
+    }
+//    std::cout << "*************************************" << std::endl;
+    elt = config->NextSiblingElement("map");
+  }
+
+
+  ROS_INFO("BaseControllerPos:: kp_speed %f",kp_speed_);
+  ROS_INFO("BaseControllerPos:: kp_caster_steer  %f",caster_steer_vel_gain_);
+  ROS_INFO("BaseControllerPos:: timeout %f",timeout_);
+  ROS_INFO("BaseControllerPos:: max_x_dot %f",(max_vel_.x));
+  ROS_INFO("BaseControllerPos:: max_y_dot %f",(max_vel_.y));
+  ROS_INFO("BaseControllerPos:: max_yaw_dot %f",(max_vel_.z));
+  ROS_INFO("BaseControllerPos:: max_x_accel %f",(max_accel_.x));
+  ROS_INFO("BaseControllerPos:: max_y_accel %f",(max_accel_.y));
+  ROS_INFO("BaseControllerPos:: max_yaw_accel %f",(max_accel_.z));
+
+  init(jcp_vec,robot_state);
+
+  ROS_INFO("BaseControllerPos:: Initialized");
+  return true;
+}
+
+void BaseControllerPos::addParamToMap(std::string key, double *value)
+{
+  param_map_[key] = value;
+}
+
+void BaseControllerPos::getJointValues()
+{
+  bool speeds_are_valid = true;
+
+  for(int i=0; i < num_casters_; i++)
+    steer_angle_actual_[i] = base_casters_[i].joint_state_->position_;
+
+  for(int i=0; i < num_wheels_; i++)
+    wheel_speed_actual_[i] = base_wheels_[i].joint_state_->velocity_;
+
+  for(int i=0; i < num_wheels_; i++)
+    speeds_are_valid = speeds_are_valid && !isinf(wheel_speed_actual_[i]) && !isnan(wheel_speed_actual_[i]);
+
+  if(!speeds_are_valid)
+    ROS_WARN("BaseControllerPos:: input speed values are inf or nan");
+}
+
+void BaseControllerPos::computeWheelPositions()
+{
+  libTF::Vector res1;
+  double steer_angle;
+  for(int i=0; i < num_wheels_; i++)
+  {
+    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+    res1 = base_wheels_[i].pos_.rot2D(steer_angle);
+    res1 += base_wheels_[i].parent_->pos_;
+    base_wheels_position_[i] = res1;
+  }
+}
+
+void BaseControllerPos::update()
+{
+   for (int i = 0; i < num_casters_; ++i)
+   {
+      if(base_casters_[i].joint_state_ != NULL)
+      {
+         if (!base_casters_[i].joint_state_->calibrated_)
+         {
+            ROS_WARN("Casters are not calibrated");
+         }
+      }
+      else
+      {
+         ROS_WARN("joint state not initialized: %s",base_casters_[i].name_.c_str());
+      }
+   }
+
+  double current_time = robot_state_->hw_->current_time_;
+  double dT = std::min<double>(current_time - last_time_,MAX_DT_);
+
+  if((current_time - cmd_received_timestamp_) > timeout_)
+  {
+    cmd_vel_.x = 0;
+    cmd_vel_.y = 0;
+    cmd_vel_.z = 0;
+    cmd_vel_magnitude_ = 0;
+    setVelocityCmdTrajectory(cmd_vel_,max_accel_);
+  }
+  else
+     cmd_vel_ = interpolateCommand(current_time-sample_time_);
+
+
+  getJointValues();
+
+
+  computeJointCommands(current_time-sample_time_,dT);
+
+  setJointCommands();
+
+  computeOdometry(current_time);
+
+  updateJointControllers();
+
+  last_time_ = current_time;
+
+  if(new_cmd_available_)
+  {
+    if(pthread_mutex_trylock(&base_controller_lock_)==0)
+    {
+      desired_vel_.x = cmd_vel_t_.x;
+      desired_vel_.y = cmd_vel_t_.y;
+      desired_vel_.z = cmd_vel_t_.z;
+      new_cmd_available_ = false;
+      sample_time_ = current_time;
+      setVelocityCmdTrajectory(desired_vel_,max_accel_);
+      pthread_mutex_unlock(&base_controller_lock_);
+    }
+  }
+
+}
+
+libTF::Vector BaseControllerPos::interpolateCommand(double time)
+{
+   libTF::Vector result;
+
+   cmd_vel_trajectory_->sample(current_vel_point_,time);
+
+   cmd_vel_direction_.x = current_vel_point_.q_[0];
+   cmd_vel_direction_.y = current_vel_point_.q_[1];
+   cmd_vel_direction_.z = current_vel_point_.q_[2];
+   cmd_vel_magnitude_ =  current_vel_point_.q_[3];
+
+   result.x = current_vel_point_.q_[0] * current_vel_point_.q_[3];
+   result.y = current_vel_point_.q_[1] * current_vel_point_.q_[3];
+   result.z = current_vel_point_.q_[2] * current_vel_point_.q_[3];
+
+//   ROS_INFO("Interpolate command: done sampling trajectory: %f %f %f",result.x, result.y, result.z);
+
+   return result;
+}
+
+void BaseControllerPos::setVelocityCmdTrajectory(libTF::Vector new_cmd, libTF::Vector max_rate)
+{
+   double dt_min_x(0), dt_min_y(0), dt_min_t(0), dt(0);
+   double new_cmd_mag = sqrt(new_cmd.x*new_cmd.x+new_cmd.y*new_cmd.y+new_cmd.z*new_cmd.z);
+   libTF::Vector new_vel_direction;
+
+   if(max_rate.x > EPS)
+      dt_min_x = fabs(new_cmd.x-cmd_vel_.x)/max_rate.x;
+   if(max_rate.y > EPS)
+      dt_min_y = fabs(new_cmd.y-cmd_vel_.y)/max_rate.y;
+   if(max_rate.z > EPS)
+      dt_min_t = fabs(new_cmd.z-cmd_vel_.z)/max_rate.z;
+
+   dt = std::max(std::max(dt_min_x,dt_min_y),dt_min_t);
+
+//   ROS_INFO("Current command: %f %f %f",cmd_vel_.x,cmd_vel_.y,cmd_vel_.z);
+
+
+   cmd_vel_points_[0].q_[0] = cmd_vel_direction_.x;
+   cmd_vel_points_[0].q_[1] = cmd_vel_direction_.y;
+   cmd_vel_points_[0].q_[2] = cmd_vel_direction_.z;
+   cmd_vel_points_[0].q_[3] = cmd_vel_magnitude_;
+   cmd_vel_points_[0].time_ = 0.0;
+
+   if(new_cmd_mag > EPS)
+   {
+      new_vel_direction.x = new_cmd.x/new_cmd_mag;
+      new_vel_direction.y = new_cmd.y/new_cmd_mag;
+      new_vel_direction.z = new_cmd.z/new_cmd_mag;
+   }
+   else
+   {
+      new_vel_direction.x = cmd_vel_direction_.x;
+      new_vel_direction.y = cmd_vel_direction_.y;
+      new_vel_direction.z = cmd_vel_direction_.z; 
+   }
+
+   cmd_vel_points_[1].q_[0] = new_vel_direction.x;
+   cmd_vel_points_[1].q_[1] = new_vel_direction.y;
+   cmd_vel_points_[1].q_[2] = new_vel_direction.z;
+   cmd_vel_points_[1].q_[3] = new_cmd_mag;
+   cmd_vel_points_[1].time_ = dt;
+
+   cmd_vel_trajectory_->setTrajectory(cmd_vel_points_);
+
+//   ROS_INFO("New command: %f %f %f",new_cmd.x,new_cmd.y,new_cmd.z);
+//   ROS_INFO("Max rate   : %f %f %f",max_rate.x,max_rate.y,max_rate.z);
+//   ROS_INFO("New command issued: %f",dt);
+}
+
+void BaseControllerPos::computeJointCommands(double current_sample_time, double dT)
+{
+  computeWheelPositions();
+
+  computeDesiredCasterSteer(current_sample_time, dT);
+
+  computeDesiredWheelSpeeds();
+}
+
+void BaseControllerPos::setJointCommands()
+{
+  setDesiredCasterSteer();
+
+  setDesiredWheelSpeeds();
+}
+
+void BaseControllerPos::computeDesiredCasterSteer(double current_sample_time, double dT)
+{
+   libTF::Vector result, result_dt;
+   libTF::Vector cmd_vel_dt;
+
+   cmd_vel_dt = interpolateCommand(current_sample_time+dT);
+
+  double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
+  double error_steer(0.0),error_steer_m_pi(0.0);
+
+  double steer_angle_desired_dt(0.0), steer_angle_desired_m_pi_dt(0.0);
+  double error_steer_dt(0.0),error_steer_m_pi_dt(0.0);
+  double trans_vel = sqrt(cmd_vel_.x * cmd_vel_.x + cmd_vel_.y * cmd_vel_.y);
+
+  for(int i=0; i < num_casters_; i++)
+  {
+    result    = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
+    result_dt = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_dt);
+
+    if( trans_vel < CMD_VEL_TRANS_EPS && fabs(cmd_vel_.z) < CMD_VEL_ROT_EPS)
+    {
+      steer_angle_desired = steer_angle_stored_[i];
+      steer_angle_desired_dt = steer_angle_stored_[i];
+    }
+    else
+    {
+      steer_angle_desired = atan2(result.y,result.x);
+      steer_angle_desired_dt = atan2(result_dt.y,result_dt.x);
+      steer_angle_stored_[i] = steer_angle_desired;
+    }
+    steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired+M_PI);
+    steer_angle_desired_m_pi_dt = angles::normalize_angle(steer_angle_desired_dt+M_PI);
+
+    error_steer = angles::shortest_angular_distance(steer_angle_desired, steer_angle_actual_[i]);
+    error_steer_m_pi = angles::shortest_angular_distance(steer_angle_desired_m_pi, steer_angle_actual_[i]);
+
+    error_steer_dt = angles::shortest_angular_distance(steer_angle_desired_dt, steer_angle_actual_[i]);
+    error_steer_m_pi_dt = angles::shortest_angular_distance(steer_angle_desired_m_pi_dt, steer_angle_actual_[i]);
+
+    if(fabs(error_steer_m_pi) < fabs(error_steer))
+    {
+      error_steer = error_steer_m_pi;
+      steer_angle_desired = steer_angle_desired_m_pi;
+    }
+
+    if(fabs(error_steer_m_pi_dt) < fabs(error_steer_dt))
+    {
+      error_steer_dt = error_steer_m_pi_dt;
+      steer_angle_desired_dt = steer_angle_desired_m_pi_dt;
+    }
+
+    steer_position_desired_[i] = steer_angle_desired;
+    steer_velocity_desired_[i] = (steer_angle_desired_dt - steer_angle_desired)/dT - kp_speed_*error_steer;
+//    ROS_INFO("Caster speed component: %d %f %f %f",i,steer_angle_desired,steer_angle_desired_dt,steer_velocity_desired_[i]);
+  }
+}
+
+void BaseControllerPos::setDesiredCasterSteer()
+{
+  for(int i=0; i < num_casters_; i++)
+     base_casters_[i].position_controller_.setCommand(steer_position_desired_[i]);
+}
+
+void BaseControllerPos::computeDesiredWheelSpeeds()
+{
+  libTF::Vector wheel_point_velocity;
+  libTF::Vector wheel_point_velocity_projected;
+  libTF::Vector wheel_caster_steer_component;
+  libTF::Vector caster_2d_velocity;
+
+  caster_2d_velocity.x = 0;
+  caster_2d_velocity.y = 0;
+  caster_2d_velocity.z = 0;
+
+  double steer_angle_actual = 0;
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+    caster_2d_velocity.z = caster_steer_vel_gain_*steer_velocity_desired_[base_wheels_[i].parent_->local_id_];
+    steer_angle_actual = base_wheels_[i].parent_->joint_state_->position_;
+    wheel_point_velocity = computePointVelocity2D(base_wheels_position_[i],cmd_vel_);
+
+    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+    wheel_point_velocity_projected = wheel_point_velocity.rot2D(-steer_angle_actual);
+    wheel_speed_cmd_[i] = (wheel_point_velocity_projected.x + wheel_caster_steer_component.x)/(wheel_radius_);
+//    ROS_INFO("Wheel speed cmd/actual %d %f %f",i,wheel_speed_cmd_[i],wheel_speed_actual_[i]);
+  }
+}
+
+void BaseControllerPos::setDesiredWheelSpeeds()
+{
+  for(int i=0; i < (int) num_wheels_; i++)
+  {
+     base_wheels_[i].velocity_controller_.setCommand(wheel_speed_cmd_[i]);
+//     ROS_INFO("Wheel speed cmd/actual %d %f %f",i,wheel_speed_cmd_[i],wheel_speed_actual_[i]);
+  }
+}
+
+
+void BaseControllerPos::updateJointControllers()
+{
+  for(int i=0; i < num_wheels_; i++)
+    base_wheels_[i].velocity_controller_.update();
+  for(int i=0; i < num_casters_; i++)
+    base_casters_[i].position_controller_.update();
+}
+
+void BaseControllerPos::setOdomMessage(std_msgs::RobotBase2DOdom &odom_msg_)
+{
+  odom_msg_.header.frame_id   = "odom";
+  odom_msg_.header.stamp.sec  = (unsigned long)floor(robot_state_->hw_->current_time_);
+  odom_msg_.header.stamp.nsec = (unsigned long)floor(  1e9 * (  robot_state_->hw_->current_time_ - odom_msg_.header.stamp.sec) );
+
+  odom_msg_.pos.x  = base_odom_position_.x;
+  odom_msg_.pos.y  = base_odom_position_.y;
+  odom_msg_.pos.th = angles::normalize_angle(base_odom_position_.z);
+
+  odom_msg_.vel.x  = base_odom_velocity_.x;
+  odom_msg_.vel.y  = base_odom_velocity_.y;
+  odom_msg_.vel.th = base_odom_velocity_.z;
+}
+
+
+void BaseControllerPos::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
+{
+  x = base_odom_position_.x;
+  y = base_odom_position_.y;
+  w = base_odom_position_.z;
+
+  vx = base_odom_velocity_.x;
+  vy = base_odom_velocity_.y;
+  vw = base_odom_velocity_.z;
+}
+
+
+Vector BaseControllerPos::computePointVelocity2D(const Vector& pos, const Vector& vel)
+{
+  Vector result;
+
+  result.x = vel.x - pos.y * vel.z;
+  result.y = vel.y + pos.x * vel.z;
+  result.z = 0;
+
+  return result;
+}
+
+void BaseControllerPos::computeOdometry(double time)
+{
+  double dt = time-last_time_;
+
+  computeBaseVelocity();
+
+  libTF::Vector base_odom_delta = (base_odom_velocity_*dt).rot2D(base_odom_position_.z);
+
+  base_odom_delta.z = base_odom_velocity_.z * dt;
+  base_odom_position_ += base_odom_delta;
+
+  odometer_distance_ += sqrt(base_odom_delta.x*base_odom_delta.x + base_odom_delta.y*base_odom_delta.y);
+  odometer_angle_ += base_odom_delta.z;
+}
+
+void BaseControllerPos::computeBaseVelocity()
+{
+  Matrix A(2*num_wheels_,1);
+  Matrix C(2*num_wheels_,3);
+  Matrix D(3,1);
+  double steer_angle;
+  double wheel_speed;
+  libTF::Vector caster_2d_velocity;
+  libTF::Vector wheel_caster_steer_component;
+
+  caster_2d_velocity.x = 0;
+  caster_2d_velocity.y = 0;
+
+  for(int i = 0; i < num_wheels_; i++) {
+    caster_2d_velocity.z = base_wheels_[i].parent_->joint_state_->velocity_;
+    wheel_caster_steer_component = computePointVelocity2D(base_wheels_[i].pos_,caster_2d_velocity);
+    wheel_speed = wheel_speed_actual_[i]-wheel_caster_steer_component.x/(wheel_radius_);
+
+    steer_angle = base_wheels_[i].parent_->joint_state_->position_;
+
+//    A.element(i*2,0)   = cos(steer_angle)*wheel_radius_*wheel_speed;
+//    A.element(i*2+1,0) = sin(steer_angle)*wheel_radius_*wheel_speed;
+
+    A.element(i*2,0)   = wheel_radius_*wheel_speed;
+    A.element(i*2+1,0) = 0;
+
+
+//  }
+//  for(int i = 0; i < num_wheels_; i++) {
+
+    C.element(i*2, 0)   = cos(steer_angle);
+    C.element(i*2, 1)   = sin(steer_angle);
+    C.element(i*2, 2)   = -cos(steer_angle) * base_wheels_position_[i].y + sin(steer_angle) *  base_wheels_position_[i].x;
+    C.element(i*2+1, 0)   = -sin(steer_angle);
+    C.element(i*2+1, 1)   = cos(steer_angle);
+    C.element(i*2+1, 2)   = sin(steer_angle) * base_wheels_position_[i].y + cos(steer_angle) *  base_wheels_position_[i].x;
+  }
+  //   D = pseudoInverse(C)*A;
+  D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
+  base_odom_velocity_.x = (double)D.element(0,0);
+  base_odom_velocity_.y = (double)D.element(1,0);
+  base_odom_velocity_.z = (double)D.element(2,0);
+}
+
+Matrix BaseControllerPos::pseudoInverse(const Matrix M)
+{
+  Matrix result;
+  //int rows = this->rows();
+  //int cols = this->columns();
+  // calculate SVD decomposition
+  Matrix U,V;
+  DiagonalMatrix D;
+  NEWMAT::SVD(M,D,U,V, true, true);
+  Matrix Dinv = D.i();
+  result = V * Dinv * U.t();
+  return result;
+}
+
+/*std::ostream & controller::operator<<(std::ostream& mystream, const controller::BaseParam &bp)
+{
+  mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
+  return mystream;
+  };*/
+
+NEWMAT::Matrix BaseControllerPos::iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter)
+{
+  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+  NEWMAT::Matrix weight_matrix(2*num_wheels_,2*num_wheels_);
+  NEWMAT::Matrix a_fit(2*num_wheels_,3);
+  NEWMAT::Matrix b_fit(2*num_wheels_,1);
+  NEWMAT::Matrix residual(2*num_wheels_,1);
+  NEWMAT::Matrix x_fit(3,1);
+
+  weight_matrix = ident_matrix;
+
+  for(int i=0; i < max_iter; i++)
+  {
+    a_fit = weight_matrix * A;
+    b_fit = weight_matrix * b;
+    x_fit = pseudoInverse(a_fit)*b_fit;
+    residual = b - A * x_fit;
+
+    for(int j=1; j <= num_wheels_; j++)
+    {
+      int fw = 2*j-1;
+      int sw = fw+1;
+      if(fabs(residual(fw,1)) > fabs(residual(sw,1)))
+      {
+        residual(fw,1) = fabs(residual(fw,1));
+        residual(sw,1) = residual(fw,1);
+      }
+      else
+      {
+        residual(fw,1) = fabs(residual(sw,1));
+        residual(sw,1) = residual(fw,1);
+      }
+    }
+    weight_matrix = findWeightMatrix(residual,weight_type);
+  }
+  return x_fit;
+};
+
+NEWMAT::Matrix BaseControllerPos::findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type)
+{
+  NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
+  NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
+
+  w_fit = ident_matrix;
+  double epsilon = 0;
+  double g_sigma = 0.1;
+
+  if(weight_type == std::string("BubeLagan"))
+  {
+    for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+    {
+      if(fabs(residual(i,1) > epsilon))
+        epsilon = fabs(residual(i,1));
+    }
+    epsilon = epsilon/100.0;
+  }
+  for(int i=1; i <= 2*num_wheels_; i++) //NEWMAT indexing starts from 1
+  {
+    if(weight_type == std::string("L1norm"))
+    {
+      w_fit(i,i) = 1.0/(1 + sqrt(fabs(residual(i,1))));
+    }
+    else if(weight_type == std::string("fair"))
+    {
+      w_fit(i,i) = 1.0/(1 + fabs(residual(i,1)));
+    }
+    else if(weight_type == std::string("Cauchy"))
+    {
+      w_fit(i,i) = 1.0/(1 + residual(i,1)*residual(i,1));
+    }
+    else if(weight_type == std::string("BubeLangan"))
+    {
+      w_fit(i,i) = 1.0/pow((1 + pow((residual(i,1)/epsilon),2)),0.25);
+    }
+    else if(weight_type == std::string("Gaussian"))
+    {
+      w_fit(i,i) = sqrt(exp(-pow(residual(i,1),2)/(2*g_sigma*g_sigma)));
+    }
+    else // default to fair
+    {
+      w_fit(i,i) = 1.0/(0.1 + sqrt(fabs(residual(i,1))));
+    }
+  }
+  return w_fit;
+};
+
+
+ROS_REGISTER_CONTROLLER(BaseControllerPosNode)
+
+  BaseControllerPosNode::BaseControllerPosNode()
+{
+  c_ = new BaseControllerPos();
+  node = ros::node::instance();
+  last_time_message_sent_ = 0.0;
+  odom_publish_rate_ = 100.0;
+  odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+  publisher_ = NULL;
+  transform_publisher_ = NULL;
+  odometer_publisher_ = NULL;
+}
+
+BaseControllerPosNode::~BaseControllerPosNode()
+{
+  node->unadvertise_service(service_prefix + "/set_command");
+  node->unadvertise_service(service_prefix + "/get_actual");
+  node->unadvertise_service(service_prefix + "/set_wheel_radius_multiplier");
+  node->unadvertise_service(service_prefix + "/get_wheel_radius_multiplier");
+
+  node->unsubscribe("cmd_vel");
+
+  publisher_->stop();
+  transform_publisher_->stop();
+  delete publisher_;
+  delete transform_publisher_;
+  delete odometer_publisher_;
+  delete c_;
+}
+
+void BaseControllerPosNode::update()
+{
+  double time = c_->robot_state_->hw_->current_time_;
+//  ROS_INFO("In Base Controller Node");  
+  c_->update();
+  c_->setOdomMessage(odom_msg_);
+
+  if (time-last_time_message_sent_ >= odom_publish_delta_t_) // send odom message
+  {
+
+    if (odometer_publisher_->trylock())
+    {
+      odometer_publisher_->msg_.distance = c_->odometer_distance_; 
+      odometer_publisher_->msg_.angle = c_->odometer_angle_; 
+      odometer_publisher_->unlockAndPublish();      
+    }
+
+    if (publisher_->trylock())
+    {
+      c_->setOdomMessage(publisher_->msg_);
+      publisher_->unlockAndPublish() ;
+      last_time_message_sent_ = time;
+    }
+
+    if (transform_publisher_->trylock())
+    {
+      double x=0,y=0,yaw=0,vx,vy,vyaw;
+      this->getOdometry(x,y,yaw,vx,vy,vyaw);
+      tf::TransformEuler &out = transform_publisher_->msg_.eulers[0];
+      out.header.stamp.from_double(time);
+      out.header.frame_id = "odom";
+      out.parent = "base_footprint";
+      out.x = -x*cos(yaw) - y*sin(yaw);
+      out.y = +x*sin(yaw) - y*cos(yaw);
+      out.z = 0;
+      out.roll = 0;
+      out.pitch = 0;
+      out.yaw = angles::normalize_angle(-yaw);
+
+
+      tf::TransformEuler &out2 = transform_publisher_->msg_.eulers[1];
+      out2.header.stamp.from_double(time);
+      out2.header.frame_id = "base_footprint";
+      out2.parent = "base_link";
+      out2.x = 0;
+      out2.y = 0;
+      out2.z = -c_->wheel_radius_;
+      out2.roll = 0;
+      out2.pitch = 0;
+      out2.yaw = 0;
+
+      transform_publisher_->unlockAndPublish() ;
+    }
+  }
+}
+
+bool BaseControllerPosNode::setCommand(
+  pr2_mechanism_controllers::SetBaseCommand::request &req,
+  pr2_mechanism_controllers::SetBaseCommand::response &resp)
+{
+  libTF::Vector command;
+  command.x = req.vx;
+  command.y = req.vy;
+  command.z = req.vw;
+  c_->setCommand(command);
+  command = c_->getCommand();
+  resp.vx = command.x;
+  resp.vy = command.y;
+  resp.vw = command.z;
+  return true;
+}
+
+void BaseControllerPosNode::setCommand(double vx, double vy, double vw)
+{
+  libTF::Vector command;
+  command.x = vx;
+  command.y = vy;
+  command.z = vw;
+  c_->setCommand(command);
+  //std::cout << "BaseControllerPos:: odom_publish_rate: " << odom_publish_rate_ << endl;
+}
+
+
+bool BaseControllerPosNode::getCommand(
+  pr2_mechanism_controllers::GetBaseCommand::request &req,
+  pr2_mechanism_controllers::GetBaseCommand::response &resp)
+{
+  libTF::Vector command;
+  command = c_->getCommand();
+  resp.vx = command.x;
+  resp.vy = command.y;
+  resp.vw = command.z;
+
+  return true;
+}
+
+bool BaseControllerPosNode::getWheelRadiusMultiplier(
+  pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+  pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
+{
+  double param_multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",param_multiplier,1.0);
+  resp.radius_multiplier = param_multiplier;
+
+  return true;
+}
+
+bool BaseControllerPosNode::setWheelRadiusMultiplier(
+  pr2_mechanism_controllers::WheelRadiusMultiplier::request &req,
+  pr2_mechanism_controllers::WheelRadiusMultiplier::response &resp)
+{
+  double calibration_multiplier = req.radius_multiplier;
+  ROS_INFO("Received radius multiplier %f ",calibration_multiplier); 
+  c_->wheel_radius_ *= calibration_multiplier;
+
+  double param_multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",param_multiplier,1.0);
+
+  node->set_param("base_controller/wheel_radius_multiplier",param_multiplier*calibration_multiplier);
+
+  return true;
+}
+
+
+bool BaseControllerPosNode::initXml(mechanism::RobotState *robot_state, TiXmlElement *config)
+{
+  service_prefix = config->Attribute("name");
+
+  ROS_INFO("Initializing base controller node");
+
+  assert(robot_state); //this happens, see pr ticket 351,but not sure why yet
+
+  if(!c_->initXml(robot_state, config))
+    return false;
+
+  ROS_INFO("Initialized base controller");
+
+  node->advertise_service(service_prefix + "/set_wheel_radius_multiplier", &BaseControllerPosNode::setWheelRadiusMultiplier,this);
+  node->advertise_service(service_prefix + "/get_wheel_radius_multiplier", &BaseControllerPosNode::getWheelRadiusMultiplier,this);
+
+
+  node->advertise_service(service_prefix + "/set_command", &BaseControllerPosNode::setCommand, this);
+  node->advertise_service(service_prefix + "/get_command", &BaseControllerPosNode::getCommand, this); //FIXME: this is actually get command, just returning command for testing.
+
+  node->advertise_service(service_prefix + "/set_command", &BaseControllerPosNode::setCommand, this);
+  node->subscribe("cmd_vel", baseVelMsg, &BaseControllerPosNode::CmdBaseVelReceived, this,1);
+
+
+  if (publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete publisher_ ;
+  publisher_ = new misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom> ("odom", 1) ;
+
+  if (odometer_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete odometer_publisher_ ;
+  odometer_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
+
+  if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete transform_publisher_ ;
+  transform_publisher_ = new misc_utils::RealtimePublisher <tf::TransformArray> ("TransformArray", 5) ;
+
+  node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
+
+  double multiplier;
+  node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
+  c_->wheel_radius_ = c_->wheel_radius_*multiplier;
+
+  transform_publisher_->msg_.set_eulers_size(NUM_TRANSFORMS);
+
+  if(odom_publish_rate_ > 1e-5)
+    odom_publish_delta_t_ = 1.0/odom_publish_rate_;
+
+  ROS_INFO("Initialized base controller node");
+
+  return true;
+}
+
+void BaseControllerPosNode::CmdBaseVelReceived()
+{
+  this->ros_lock_.lock();
+  this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+  this->ros_lock_.unlock();
+}
+
+void BaseControllerPosNode::getOdometry(double &x, double &y, double &w, double &vx, double &vy, double &vw)
+{
+  c_->getOdometry(x,y,w,vx,vy,vw);
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7929
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7929&view=rev
Author:   meeussen
Date:     2008-12-10 22:32:15 +0000 (Wed, 10 Dec 2008)

Log Message:
-----------

Added covariance calculation

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-10 22:21:36 UTC (rev 7928)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-10 22:32:15 UTC (rev 7929)
@@ -57,6 +57,7 @@
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/BaseVel.h>
 #include <pr2_msgs/Odometer.h>
+#include <pr2_msgs/Covariance2D.h>
 
 #include <misc_utils/realtime_publisher.h>
 
@@ -406,6 +407,8 @@
 
     double odometer_angle_;
 
+    double odometry_residual_max_;
+
     friend class BaseControllerPosNode;
   };
 
@@ -497,11 +500,8 @@
 
     misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
-  };
+    misc_utils::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
-    /** \brief A namespace ostream overload for displaying parameters */
-//  std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
-
     /*
      * \brief pointer to ros node
      */
@@ -510,6 +510,12 @@
      * \brief save service name prefix for unadvertise on exit
      */
     std::string service_prefix;
+
+  };
+
+    /** \brief A namespace ostream overload for displaying parameters */
+//  std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
+
 }
 
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-10 22:21:36 UTC (rev 7928)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-10 22:32:15 UTC (rev 7929)
@@ -42,6 +42,8 @@
 #define CMD_VEL_TRANS_EPS 1e-5
 #define CMD_VEL_ROT_EPS 1e-5
 
+static const double MIN_BASE_CONTROLLER_COVARIANCE = 0.00001;
+
 using namespace ros;
 using namespace std;
 using namespace controller;
@@ -685,6 +687,8 @@
   libTF::Vector caster_2d_velocity;
   libTF::Vector wheel_caster_steer_component;
 
+  NEWMAT::Matrix odometry_residual(2*num_wheels_,1);
+
   caster_2d_velocity.x = 0;
   caster_2d_velocity.y = 0;
 
@@ -714,6 +718,10 @@
   }
   //   D = pseudoInverse(C)*A;
   D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
+
+  odometry_residual = C*D-A;
+  odometry_residual_max_ = odometry_residual.MaximumAbsoluteValue();
+
   base_odom_velocity_.x = (double)D.element(0,0);
   base_odom_velocity_.y = (double)D.element(1,0);
   base_odom_velocity_.z = (double)D.element(2,0);
@@ -838,6 +846,7 @@
   publisher_ = NULL;
   transform_publisher_ = NULL;
   odometer_publisher_ = NULL;
+  covariance_publisher_ = NULL;
 }
 
 BaseControllerPosNode::~BaseControllerPosNode()
@@ -851,6 +860,9 @@
 
   publisher_->stop();
   transform_publisher_->stop();
+  odometer_publisher_->stop();
+  covariance_publisher_->stop();
+
   delete publisher_;
   delete transform_publisher_;
   delete odometer_publisher_;
@@ -881,6 +893,36 @@
       last_time_message_sent_ = time;
     }
 
+    if (covariance_publisher_->trylock())
+    {
+      double base_odom_velocity_mag = sqrt(c_->base_odom_velocity_.x*c_->base_odom_velocity_.x+c_->base_odom_velocity_.y*c_->base_odom_velocity_.y+c_->base_odom_velocity_.z*c_->base_odom_velocity_.z);
+      double dirn_x(0),dirn_y(0),dirn_z(0);
+
+      if (base_odom_velocity_mag > EPS)
+      {
+        dirn_x = fabs(c_->base_odom_velocity_.x/base_odom_velocity_mag);
+        dirn_y = fabs(c_->base_odom_velocity_.y/base_odom_velocity_mag);   
+        dirn_z = fabs(c_->base_odom_velocity_.z/base_odom_velocity_mag);
+      }
+      else
+      {
+        dirn_x = 0.0;
+        dirn_y = 0.0;
+        dirn_z = 0.0;
+      }
+      covariance_publisher_->msg_.Cxx = c_->odometry_residual_max_;
+
+      covariance_publisher_->msg_.Cxx = std::max<double>(c_->odometry_residual_max_*dirn_x,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cyy = std::max<double>(c_->odometry_residual_max_*dirn_y,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Czz = std::max<double>(c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+
+      covariance_publisher_->msg_.Cxy = std::max<double>(c_->odometry_residual_max_*dirn_x+c_->odometry_residual_max_*dirn_y,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cxz = std::max<double>(c_->odometry_residual_max_*dirn_x+c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+      covariance_publisher_->msg_.Cyz = std::max<double>(c_->odometry_residual_max_*dirn_y+c_->odometry_residual_max_*dirn_z,MIN_BASE_CONTROLLER_COVARIANCE);
+
+      covariance_publisher_->unlockAndPublish() ;
+    }
+
     if (transform_publisher_->trylock())
     {
       double x=0,y=0,yaw=0,vx,vy,vyaw;
@@ -1017,6 +1059,10 @@
     delete transform_publisher_ ;
   transform_publisher_ = new misc_utils::RealtimePublisher <tf::TransformArray> ("TransformArray", 5) ;
 
+  if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete covariance_publisher_ ;
+  covariance_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
+
   node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
 
   double multiplier;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7942
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7942&view=rev
Author:   sachinchitta
Date:     2008-12-11 01:27:01 +0000 (Thu, 11 Dec 2008)

Log Message:
-----------
added message to transmit odometry residuals

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-11 00:00:45 UTC (rev 7941)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-11 01:27:01 UTC (rev 7942)
@@ -46,6 +46,7 @@
 #include <pr2_mechanism_controllers/SetBaseCommand.h>
 #include <pr2_mechanism_controllers/GetBaseCommand.h>
 #include <pr2_mechanism_controllers/WheelRadiusMultiplier.h>
+#include <pr2_mechanism_controllers/OdometryResiduals.h>
 
 #include <libTF/Pose3D.h>
 #include <urdf/URDF.h>
@@ -276,6 +277,8 @@
 
     private:
 
+    std::vector<double> odometry_residuals_vector_;
+
     bool new_cmd_available_; /** true when new command received by node */
 
     /*!
@@ -502,6 +505,8 @@
 
     misc_utils::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
+    misc_utils::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals>* residuals_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+
     /*
      * \brief pointer to ros node
      */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-11 00:00:45 UTC (rev 7941)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-11 01:27:01 UTC (rev 7942)
@@ -230,6 +230,8 @@
       }
    }
 
+   odometry_residuals_vector_.resize(num_wheels_);
+
    for(int i =0; i < num_wheels_; i++)
    {
       link = urdf_model_.getJointLink(base_wheels_[i].name_);
@@ -720,6 +722,11 @@
   D = iterativeLeastSquares(C,A,ils_weight_type_,ils_max_iterations_);
 
   odometry_residual = C*D-A;
+  for(int i=0; i< num_wheels_; i++)
+  {
+    odometry_residuals_vector_[i] = odometry_residual(i+1,1);
+  }
+
   odometry_residual_max_ = odometry_residual.MaximumAbsoluteValue();
 
   base_odom_velocity_.x = (double)D.element(0,0);
@@ -847,6 +854,7 @@
   transform_publisher_ = NULL;
   odometer_publisher_ = NULL;
   covariance_publisher_ = NULL;
+  residuals_publisher_ = NULL;
 }
 
 BaseControllerPosNode::~BaseControllerPosNode()
@@ -859,13 +867,17 @@
   node->unsubscribe("cmd_vel");
 
   publisher_->stop();
+  odometer_publisher_->stop();
+  residuals_publisher_->stop();
   transform_publisher_->stop();
-  odometer_publisher_->stop();
   covariance_publisher_->stop();
 
   delete publisher_;
+  delete residuals_publisher_;
+  delete odometer_publisher_;
   delete transform_publisher_;
-  delete odometer_publisher_;
+  delete covariance_publisher_;
+
   delete c_;
 }
 
@@ -893,6 +905,17 @@
       last_time_message_sent_ = time;
     }
 
+    if (residuals_publisher_->trylock())
+    {
+      for(int i = 0; i < c_->num_wheels_ ; i++)
+      {
+        residuals_publisher_->msg_.residuals[i] = c_->odometry_residuals_vector_[i];
+        residuals_publisher_->msg_.names[i] = (std::string) c_->base_wheels_[i].name_;
+      }        
+      residuals_publisher_->msg_.time = time;
+      residuals_publisher_->unlockAndPublish() ;
+    }
+
     if (covariance_publisher_->trylock())
     {
       double base_odom_velocity_mag = sqrt(c_->base_odom_velocity_.x*c_->base_odom_velocity_.x+c_->base_odom_velocity_.y*c_->base_odom_velocity_.y+c_->base_odom_velocity_.z*c_->base_odom_velocity_.z);
@@ -1063,6 +1086,10 @@
     delete covariance_publisher_ ;
   covariance_publisher_ = new misc_utils::RealtimePublisher <pr2_msgs::Covariance2D> (service_prefix + "/odometry_covariance", 1) ;
 
+  if (residuals_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete residuals_publisher_ ;
+  residuals_publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::OdometryResiduals> (service_prefix + "/odometry_residuals", 1) ;
+
   node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
 
   double multiplier;
@@ -1070,6 +1097,8 @@
   c_->wheel_radius_ = c_->wheel_radius_*multiplier;
 
   transform_publisher_->msg_.set_eulers_size(NUM_TRANSFORMS);
+  residuals_publisher_->msg_.set_names_size(c_->num_wheels_);
+  residuals_publisher_->msg_.set_residuals_size(c_->num_wheels_);
 
   if(odom_publish_rate_ > 1e-5)
     odom_publish_delta_t_ = 1.0/odom_publish_rate_;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 7992
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7992&view=rev
Author:   meeussen
Date:     2008-12-11 23:34:08 +0000 (Thu, 11 Dec 2008)

Log Message:
-----------
Working version of new controller (Sachin from Wim's account)

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-11 22:46:17 UTC (rev 7991)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-11 23:34:08 UTC (rev 7992)
@@ -227,7 +227,7 @@
      * specified by max_rate
      * \param dT - timestep
      */
-    libTF::Vector interpolateCommand(double time);
+    libTF::Vector interpolateCommand(double time, bool set);
 
     /*!
      * \brief iterative least squares implementation to compute odometry. This implementation computes and returns x where A*x = b
@@ -269,6 +269,8 @@
 
     libTF::Vector cmd_vel_direction_;
 
+    libTF::Vector cmd_vel_dt_direction_;
+
     double cmd_vel_magnitude_;
 
     double sample_time_;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-11 22:46:17 UTC (rev 7991)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-11 23:34:08 UTC (rev 7992)
@@ -401,7 +401,7 @@
     setVelocityCmdTrajectory(cmd_vel_,max_accel_);
   }
   else
-     cmd_vel_ = interpolateCommand(current_time-sample_time_);
+    cmd_vel_ = interpolateCommand(current_time-sample_time_,true);
 
 
   getJointValues();
@@ -433,22 +433,30 @@
 
 }
 
-libTF::Vector BaseControllerPos::interpolateCommand(double time)
+libTF::Vector BaseControllerPos::interpolateCommand(double time, bool set)
 {
    libTF::Vector result;
 
    cmd_vel_trajectory_->sample(current_vel_point_,time);
-
+   if(set)
+     {
    cmd_vel_direction_.x = current_vel_point_.q_[0];
    cmd_vel_direction_.y = current_vel_point_.q_[1];
    cmd_vel_direction_.z = current_vel_point_.q_[2];
    cmd_vel_magnitude_ =  current_vel_point_.q_[3];
+     }
+   else
+     {
+       cmd_vel_dt_direction_.x = current_vel_point_.q_[0];
+       cmd_vel_dt_direction_.y = current_vel_point_.q_[1];
+       cmd_vel_dt_direction_.z = current_vel_point_.q_[2];
+     }
 
    result.x = current_vel_point_.q_[0] * current_vel_point_.q_[3];
    result.y = current_vel_point_.q_[1] * current_vel_point_.q_[3];
    result.z = current_vel_point_.q_[2] * current_vel_point_.q_[3];
 
-   ROS_INFO("Interpolate command: done sampling trajectory: %f %f %f",result.x, result.y, result.z);
+   //   ROS_INFO("Interpolate command: done sampling trajectory: %f %f %f",result.x, result.y, result.z);
 
    return result;
 }
@@ -496,7 +504,7 @@
    }
    else
    {
-     ROS_INFO("Keeping the old trajectory %f %f %f",cmd_vel_direction_.x,cmd_vel_direction_.y,cmd_vel_direction_.z);
+     //     ROS_INFO("Keeping the old trajectory %f %f %f",cmd_vel_direction_.x,cmd_vel_direction_.y,cmd_vel_direction_.z);
       new_vel_direction.x = cmd_vel_direction_.x;
       new_vel_direction.y = cmd_vel_direction_.y;
       new_vel_direction.z = cmd_vel_direction_.z; 
@@ -536,8 +544,19 @@
    libTF::Vector result, result_dt;
    libTF::Vector cmd_vel_dt;
 
-   cmd_vel_dt = interpolateCommand(current_sample_time+dT);
+   libTF::Vector cmd_vel_dirn;
+   libTF::Vector cmd_vel_dirn_dt;
 
+   cmd_vel_dirn.x = cmd_vel_direction_.x;
+   cmd_vel_dirn.y = cmd_vel_direction_.y;
+   cmd_vel_dirn.z = cmd_vel_direction_.z;
+
+   cmd_vel_dt = interpolateCommand(current_sample_time+dT,false);
+
+   cmd_vel_dirn_dt.x = cmd_vel_dt_direction_.x;
+   cmd_vel_dirn_dt.y = cmd_vel_dt_direction_.y;
+   cmd_vel_dirn_dt.z = cmd_vel_dt_direction_.z;
+
   double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
   double error_steer(0.0),error_steer_m_pi(0.0);
 
@@ -547,8 +566,8 @@
 
   for(int i=0; i < num_casters_; i++)
   {
-    result    = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_);
-    result_dt = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_dt);
+    result    = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_dirn);
+    result_dt = computePointVelocity2D(base_casters_[i].pos_, cmd_vel_dirn_dt);
 
     if( trans_vel < CMD_VEL_TRANS_EPS && fabs(cmd_vel_.z) < CMD_VEL_ROT_EPS)
     {
@@ -570,7 +589,7 @@
     error_steer_dt = angles::shortest_angular_distance(steer_angle_desired_dt, steer_angle_actual_[i]);
     error_steer_m_pi_dt = angles::shortest_angular_distance(steer_angle_desired_m_pi_dt, steer_angle_actual_[i]);
 
-    if(fabs(error_steer_m_pi) < fabs(error_steer))
+    /*    if(fabs(error_steer_m_pi) < fabs(error_steer))
     {
       error_steer = error_steer_m_pi;
       steer_angle_desired = steer_angle_desired_m_pi;
@@ -581,10 +600,10 @@
       error_steer_dt = error_steer_m_pi_dt;
       steer_angle_desired_dt = steer_angle_desired_m_pi_dt;
     }
-
+    */
     steer_position_desired_[i] = steer_angle_desired;
     steer_velocity_desired_[i] = (steer_angle_desired_dt - steer_angle_desired)/dT - kp_speed_*error_steer;
-    ROS_INFO("Caster position desired: %d %f ",i,steer_position_desired_[i]);
+    //    ROS_INFO("Caster position desired: %d %f ",i,steer_position_desired_[i]);
   }
 }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 8032
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8032&view=rev
Author:   sachinchitta
Date:     2008-12-12 22:36:17 +0000 (Fri, 12 Dec 2008)

Log Message:
-----------
Modified arm_trajectory controller to impose limits on startup as well. 

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2008-12-12 21:33:33 UTC (rev 8031)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2008-12-12 22:36:17 UTC (rev 8032)
@@ -103,14 +103,17 @@
   trajectory_point_.setDimension((int) joint_position_controllers_.size());
   dimension_ = (int) joint_position_controllers_.size();
 
+
+// Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
+  for(int j=0; j < dimension_; j++)
+    trajectory_point_.q_[j] = joint_position_controllers_[j]->joint_state_->position_;
+  trajectory_point_.time_ = 0.0;
+  trajectory_points_vector.push_back(trajectory_point_);
+
   for(int i=0; i < dimension_; i++)
      trajectory_point_.q_[i] = 0.0;
-
   trajectory_point_.time_ = 0.0;
-
-// Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
   trajectory_points_vector.push_back(trajectory_point_);
-  trajectory_points_vector.push_back(trajectory_point_);
 
   joint_trajectory_->autocalc_timing_ = true;
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2008-12-12 21:33:33 UTC (rev 8031)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2008-12-12 22:36:17 UTC (rev 8032)
@@ -58,7 +58,7 @@
   pr2_mechanism_controllers::JointTraj cmd;
 
   int num_points = 2;
-  int num_joints = 5;
+  int num_joints = 7;
 
   cmd.set_points_size(num_points);
 
@@ -70,13 +70,17 @@
   cmd.points[0].positions[2] = 0.0;
   cmd.points[0].positions[3] = 0.0;
   cmd.points[0].positions[4] = 0.0;
+  cmd.points[0].positions[5] = 0.0;
+  cmd.points[0].positions[6] = 0.0;
   cmd.points[0].time = 0.0;
 
   cmd.points[1].positions[0] = 1.0;
   cmd.points[1].positions[1] = 1.0;
   cmd.points[1].positions[2] = 0.2;
-  cmd.points[1].positions[3] = 0.5;
+  cmd.points[1].positions[3] = -0.5;
   cmd.points[1].positions[4] = 0.4;
+  cmd.points[1].positions[5] = 0.0;
+  cmd.points[1].positions[6] = 0.0;
   cmd.points[1].time = 10.0;
 
   node->advertise<pr2_mechanism_controllers::JointTraj>("arm_trajectory_command",1);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 8034
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8034&view=rev
Author:   sachinchitta
Date:     2008-12-12 22:40:11 +0000 (Fri, 12 Dec 2008)

Log Message:
-----------
scaled velocity limits more aggressively

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-12-12 22:38:21 UTC (rev 8033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2008-12-12 22:40:11 UTC (rev 8034)
@@ -9,6 +9,7 @@
 
 rospack_add_library( pr2_mechanism_controllers
   src/base_controller.cpp
+  src/base_controller_pos.cpp
   src/base_position_controller.cpp
   src/laser_scanner_controller.cpp
   src/laser_scanner_traj_controller.cpp
@@ -34,8 +35,8 @@
 #rospack_add_executable(testArmDynamicsController test/test_arm_dynamics_controller.cpp)
 #target_link_libraries(testRunBaseController pr2_mechanism_controllers)
 
-#rospack_add_executable(testOdom test/test_odom.cpp)
-#target_link_libraries(testOdom pr2_mechanism_controllers)
+rospack_add_executable(testOdom test/test_odom.cpp)
+target_link_libraries(testOdom pr2_mechanism_controllers)
 
 rospack_add_executable(testTraj test/test_arm_trajectory_controller.cpp)
 target_link_libraries(testTraj pr2_mechanism_controllers)

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2008-12-12 22:38:21 UTC (rev 8033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2008-12-12 22:40:11 UTC (rev 8034)
@@ -77,7 +77,7 @@
 
     joint = (robot->getJointState(jpc->getJointName()))->joint_;
     if(joint)
-      joint_velocity_limits.push_back(joint->velocity_limit_/5.0);
+      joint_velocity_limits.push_back(joint->velocity_limit_/2.0);
 
     elt = elt->NextSiblingElement("controller");
   }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-12 22:38:21 UTC (rev 8033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-12 22:40:11 UTC (rev 8034)
@@ -800,6 +800,9 @@
 
   publisher_->stop();
   transform_publisher_->stop();
+  odometer_publisher_->stop();
+  covariance_publisher_->stop();
+
   delete publisher_;
   delete transform_publisher_;
   delete odometer_publisher_;
@@ -848,7 +851,7 @@
         dirn_y = 0.0;
         dirn_z = 0.0;
       }
-      covariance_publisher_->msg_.Cxx = c_->odometry_residual_max_;
+      //     covariance_publisher_->msg_.Cxx = c_->odometry_residual_max_;
 
       covariance_publisher_->msg_.Cxx = std::max<double>(c_->odometry_residual_max_*dirn_x,MIN_BASE_CONTROLLER_COVARIANCE);
       covariance_publisher_->msg_.Cyy = std::max<double>(c_->odometry_residual_max_*dirn_y,MIN_BASE_CONTROLLER_COVARIANCE);

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2008-12-12 22:38:21 UTC (rev 8033)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2008-12-12 22:40:11 UTC (rev 8034)
@@ -143,8 +143,6 @@
 
   node->advertise<std_msgs::BaseVel>("cmd_vel",10);
   sleep(1);
-  node->publish("cmd_vel",cmd);
-  sleep(1);
 
   libTF::Vector ang_rates;
   ros::Time start_time = ros::Time::now();


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 8288
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8288&view=rev
Author:   tfoote
Date:     2008-12-17 23:08:55 +0000 (Wed, 17 Dec 2008)

Log Message:
-----------
converting to tf_message from TransformArray

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-17 23:03:13 UTC (rev 8287)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2008-12-17 23:08:55 UTC (rev 8288)
@@ -60,7 +60,7 @@
 
 #include <misc_utils/realtime_publisher.h>
 
-#include <tf/TransformArray.h>
+#include <tf/tfMessage.h>
 
 #include <pthread.h>
 
@@ -482,7 +482,7 @@
            
     misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the odometry msg from the update() realtime loop
 
-    misc_utils::RealtimePublisher <tf::TransformArray>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+    misc_utils::RealtimePublisher <tf::tfMessage>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
     misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-17 23:03:13 UTC (rev 8287)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2008-12-17 23:08:55 UTC (rev 8288)
@@ -62,7 +62,7 @@
 
 #include <misc_utils/realtime_publisher.h>
 
-#include <tf/TransformArray.h>
+#include <tf/tfMessage.h>
 
 #include <pthread.h>
 #include <trajectory/trajectory.h>
@@ -501,7 +501,7 @@
            
     misc_utils::RealtimePublisher <std_msgs::RobotBase2DOdom>* publisher_ ;  //!< Publishes the odometry msg from the update() realtime loop
 
-    misc_utils::RealtimePublisher <tf::TransformArray>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
+    misc_utils::RealtimePublisher <tf::tfMessage>* transform_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
     misc_utils::RealtimePublisher <pr2_msgs::Odometer>* odometer_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-17 23:03:13 UTC (rev 8287)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2008-12-17 23:08:55 UTC (rev 8288)
@@ -868,28 +868,33 @@
     {
       double x=0,y=0,yaw=0,vx,vy,vyaw;
       this->getOdometry(x,y,yaw,vx,vy,vyaw);
-      tf::TransformEuler &out = transform_publisher_->msg_.eulers[0];
-      out.header.stamp.from_double(time);
+      std_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
+      out.header.stamp.fromSec(time);
       out.header.frame_id = "odom";
-      out.parent = "base_footprint";
-      out.x = -x*cos(yaw) - y*sin(yaw);
-      out.y = +x*sin(yaw) - y*cos(yaw);
-      out.z = 0;
-      out.roll = 0;
-      out.pitch = 0;
-      out.yaw = angles::normalize_angle(-yaw);
+      out.parent_id = "base_footprint";
+      out.transform.translation.x = -x*cos(yaw) - y*sin(yaw);
+      out.transform.translation.y = +x*sin(yaw) - y*cos(yaw);
+      out.transform.translation.z = 0;
+      out.transform.rotation.x = 0;
+      out.transform.rotation.y = 0;
+      double angle = angles::normalize_angle(-yaw);
+      out.transform.rotation.z = sqrt(1/(1 + pow(angle, 2)));
+      out.transform.rotation.w = sqrt(pow(angle, 2) / (1 + pow(angle, 2)));
+      //      out.pitch = 0;
+      //out.yaw = 
 
 
-      tf::TransformEuler &out2 = transform_publisher_->msg_.eulers[1];
-      out2.header.stamp.from_double(time);
+      std_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
+      out2.header.stamp.fromSec(time);
       out2.header.frame_id = "base_footprint";
-      out2.parent = "base_link";
-      out2.x = 0;
-      out2.y = 0;
-      out2.z = -c_->wheel_radius_;
-      out2.roll = 0;
-      out2.pitch = 0;
-      out2.yaw = 0;
+      out2.parent_id = "base_link";
+      out2.transform.translation.x = 0;
+      out2.transform.translation.y = 0;
+      out2.transform.translation.z = -c_->wheel_radius_;
+      out2.transform.rotation.x = 0;
+      out2.transform.rotation.y = 0;
+      out2.transform.rotation.z = 0;
+      out2.transform.rotation.w = 1;
 
       transform_publisher_->unlockAndPublish() ;
     }
@@ -994,7 +999,7 @@
 
   if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete transform_publisher_ ;
-  transform_publisher_ = new misc_utils::RealtimePublisher <tf::TransformArray> ("TransformArray", 5) ;
+  transform_publisher_ = new misc_utils::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
 
   if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete covariance_publisher_ ;
@@ -1007,7 +1012,7 @@
   node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
   c_->wheel_radius_ = c_->wheel_radius_*multiplier;
 
-  transform_publisher_->msg_.set_eulers_size(NUM_TRANSFORMS);
+  transform_publisher_->msg_.set_transforms_size(NUM_TRANSFORMS);
 
   if(odom_publish_rate_ > 1e-5)
     odom_publish_delta_t_ = 1.0/odom_publish_rate_;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-17 23:03:13 UTC (rev 8287)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2008-12-17 23:08:55 UTC (rev 8288)
@@ -981,28 +981,33 @@
     {
       double x=0,y=0,yaw=0,vx,vy,vyaw;
       this->getOdometry(x,y,yaw,vx,vy,vyaw);
-      tf::TransformEuler &out = transform_publisher_->msg_.eulers[0];
-      out.header.stamp.from_double(time);
+      std_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
+      out.header.stamp.fromSec(time);
       out.header.frame_id = "odom";
-      out.parent = "base_footprint";
-      out.x = -x*cos(yaw) - y*sin(yaw);
-      out.y = +x*sin(yaw) - y*cos(yaw);
-      out.z = 0;
-      out.roll = 0;
-      out.pitch = 0;
-      out.yaw = angles::normalize_angle(-yaw);
+      out.parent_id = "base_footprint";
+      out.transform.translation.x = -x*cos(yaw) - y*sin(yaw);
+      out.transform.translation.y = +x*sin(yaw) - y*cos(yaw);
+      out.transform.translation.z = 0;
+      out.transform.rotation.x = 0;
+      out.transform.rotation.y = 0;
+      double angle = angles::normalize_angle(-yaw);
+      out.transform.rotation.z = sqrt(1/(1 + pow(angle, 2)));
+      out.transform.rotation.w = sqrt(pow(angle, 2) / (1 + pow(angle, 2)));
+      //      out.pitch = 0;
+      //out.yaw = 
 
 
-      tf::TransformEuler &out2 = transform_publisher_->msg_.eulers[1];
-      out2.header.stamp.from_double(time);
+      std_msgs::TransformStamped &out2 = transform_publisher_->msg_.transforms[1];
+      out2.header.stamp.fromSec(time);
       out2.header.frame_id = "base_footprint";
-      out2.parent = "base_link";
-      out2.x = 0;
-      out2.y = 0;
-      out2.z = -c_->wheel_radius_;
-      out2.roll = 0;
-      out2.pitch = 0;
-      out2.yaw = 0;
+      out2.parent_id = "base_link";
+      out2.transform.translation.x = 0;
+      out2.transform.translation.y = 0;
+      out2.transform.translation.z = -c_->wheel_radius_;
+      out2.transform.rotation.x = 0;
+      out2.transform.rotation.y = 0;
+      out2.transform.rotation.z = 0;
+      out2.transform.rotation.w = 1;
 
       transform_publisher_->unlockAndPublish() ;
     }
@@ -1111,7 +1116,7 @@
 
   if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete transform_publisher_ ;
-  transform_publisher_ = new misc_utils::RealtimePublisher <tf::TransformArray> ("TransformArray", 5) ;
+  transform_publisher_ = new misc_utils::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
 
   if (covariance_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete covariance_publisher_ ;
@@ -1127,7 +1132,7 @@
   node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
   c_->wheel_radius_ = c_->wheel_radius_*multiplier;
 
-  transform_publisher_->msg_.set_eulers_size(NUM_TRANSFORMS);
+  transform_publisher_->msg_.set_transforms_size(NUM_TRANSFORMS);
   residuals_publisher_->msg_.set_names_size(c_->num_wheels_);
   residuals_publisher_->msg_.set_residuals_size(c_->num_wheels_);
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 8793
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8793&view=rev
Author:   sachinchitta
Date:     2009-01-01 01:10:24 +0000 (Thu, 01 Jan 2009)

Log Message:
-----------
pr2 specific arm dynamics controller

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_arm_dynamics_controller.h	2009-01-01 01:10:24 UTC (rev 8793)
@@ -0,0 +1,242 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+#include <mechanism_model/controller.h>
+#include <mechanism_model/joint.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+
+// Services
+#include <robot_srvs/SetJointCmd.h>
+#include <robot_srvs/GetJointCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointGains.h>
+#include <pr2_mechanism_controllers/GetJointGains.h>
+
+//Kinematics
+#include <robot_kinematics/robot_kinematics.h>
+
+// #include <libTF/Pose3D.h>
+// #include <urdf/URDF.h>
+
+namespace controller
+{
+
+// The maximum number of joints expected in an arm.
+static const int MAX_ARM_JOINTS = 7;
+
+class PR2ArmDynamicsController : public Controller
+{
+public:
+
+  /*!
+   * \brief Default Constructor of the JointController class.
+   *
+   */
+  PR2ArmDynamicsController();
+
+  /*!
+   * \brief Destructor of the JointController class.
+   */
+  virtual ~PR2ArmDynamicsController();
+
+  /*!
+   * \brief Functional way to initialize limits and gains.
+   *
+  */
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+  /*!
+   * \brief Sets a goal for some joint, specified as a pair of (joint values, joint names)
+   *
+   * \param j_values the positions of the joints
+   * \param j_names the names of the joints
+  */
+  void setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot, const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names);
+
+//  /**
+//   * @brief Overloaded method for convenience
+//   * @param req
+//   */
+//  void setJointCmd(robot_msgs::SetJointPosCmd::request &req);
+
+  void getJointCmd(robot_msgs::JointCmd & cmd) const;
+
+  /*!
+   * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+  */
+//  void getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::response &resp);
+
+//  void getCurrentConfiguration(std::vector<double> &);
+
+  /*!
+     * \brief Issues commands to the joint. Should be called at regular intervals
+  */
+  virtual void update(void); // Real time safe.
+
+  ros::thread::mutex arm_controller_lock_;
+
+//  void setJointGains(const pr2_mechanism_controllers::SetJointGains::request &req);
+
+//  void getJointGains(pr2_mechanism_controllers::GetJointGains::response &resp);
+
+  controller::JointEffortController* getJointEffortControllerByName(std::string name);
+
+  bool goalAchieved() const { return goal_achieved_; }
+
+  robot_kinematics::RobotKinematics pr2_kin_;
+
+  robot_kinematics::SerialChain *arm_chain_;
+
+  robot_kinematics::RobotKinematics pr2_kin_uncompensated_;
+
+  robot_kinematics::SerialChain *arm_chain_uncompensated_;
+
+  unsigned int num_joints_;
+
+private:
+
+  double last_time_;
+
+  std::vector<control_toolbox::Pid> pid_controllers_;
+
+  std::vector<JointEffortController *> joint_effort_controllers_;
+
+  // Goal of the joints
+  std::vector<double> goals_;
+
+  // Goal of the joints
+  std::vector<double> goals_dot_;
+
+  // Goal of the joints
+  std::vector<double> goals_dot_dot_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_dot_;
+
+  // Goal of the joints - used by the realtime code only
+  std::vector<double> goals_rt_dot_dot_;
+
+  mechanism::Robot* robot_;
+
+  void updateJointControllers(void);
+
+  int getJointControllerByName(std::string name);
+
+  void checkForGoalAchieved_(void);
+
+  bool goal_achieved_;
+
+  KDL::Vector *gravity_torque_;
+
+  KDL::Vector *gravity_torque_uncompensated_;
+
+  std::vector<double> control_torque_;
+
+  void computeControlTorque(const double &time);
+
+  // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
+  bool refresh_rt_vals_;
+
+};
+
+/** @class PR2ArmDynamicsControllerNode
+ *  @brief ROS interface for the arm controller.
+ *  @author Timothy Hunter <tjhunter@...>
+ *
+ *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
+ *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
+ *
+ */
+class PR2ArmDynamicsControllerNode : public Controller
+{
+  public:
+    /*!
+    * \brief Default Constructor
+    *
+    */
+    PR2ArmDynamicsControllerNode();
+
+    /*!
+    * \brief Destructor
+    */
+    ~PR2ArmDynamicsControllerNode();
+
+    void update();
+
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /** @brief sets a command for all the joints managed by the controller at once
+     * This is a lightweight version of setJointPosHeadless when the caller knows the order of the joints.
+     * @note this service should not be used. use setJointPosHeadless instead
+     * This service returns immediately
+     * @param req an array of position
+     * @param resp The response is empty
+     * @return
+     */
+    bool setJointSrv(robot_srvs::SetJointCmd::request &req,
+                     robot_srvs::SetJointCmd::response &resp);
+
+    /** @brief service that returns the goal of the controller
+     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
+     * @param req
+     * @param resp the response, contains a JointPosCmd message with the goal of the controller
+     * @return
+     */
+    bool getJointCmd(robot_srvs::GetJointCmd::request &req,
+                     robot_srvs::GetJointCmd::response &resp);
+
+  private:
+    robot_msgs::JointCmd msg_;   //The message used by the ROS callback
+    PR2ArmDynamicsController *c_;
+
+};
+
+}
+
+

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-01 00:19:46 UTC (rev 8792)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-01 01:10:24 UTC (rev 8793)
@@ -86,7 +86,7 @@
 
     joint = (robot->getJointState(jpc->getJointName()))->joint_;
     if(joint)
-      joint_velocity_limits.push_back(joint->velocity_limit_/2.0);
+      joint_velocity_limits.push_back(joint->velocity_limit_/1.0);
 
     elt = elt->NextSiblingElement("controller");
   }

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_arm_dynamics_controller.cpp	2009-01-01 01:10:24 UTC (rev 8793)
@@ -0,0 +1,387 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Sachin Chitta <sachinc@...>
+
+#include "pr2_mechanism_controllers/pr2_arm_dynamics_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/String.h"
+
+// Math utils
+#include <angles/angles.h>
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(PR2ArmDynamicsController);
+
+PR2ArmDynamicsController::PR2ArmDynamicsController():num_joints_(0),last_time_(0)
+{
+}
+
+PR2ArmDynamicsController::~PR2ArmDynamicsController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i < num_joints_;++i)
+  {
+    delete joint_effort_controllers_[i];
+  }
+  delete gravity_torque_;
+  delete gravity_torque_uncompensated_;
+}
+
+bool PR2ArmDynamicsController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  robot_ = robot->model_;
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    JointEffortController * jec = new JointEffortController();
+    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointEffortController"));
+    joint_effort_controllers_.push_back(jec);
+    if(!jec->initXml(robot, elt))
+      return false;
+
+    TiXmlElement *p = elt->FirstChildElement("joint")->FirstChildElement("pid");
+    if (p)
+    {
+      control_toolbox::Pid  pid_controller;
+      pid_controller.initPid(0, 0, 0, 0, 0);
+      pid_controller.initXml(p);
+      pid_controllers_.push_back(pid_controller);
+   }
+    else
+      fprintf(stderr, "JointEffortController's config did not specify the default pid parameters.\n");
+
+    elt = elt->NextSiblingElement("controller");
+  }
+
+  num_joints_ = joint_effort_controllers_.size();
+
+  fprintf(stderr,"PR2ArmDynamicsController:: num_joints_: %d\n",num_joints_);
+
+  goals_.resize(num_joints_);
+  goals_rt_.resize(num_joints_);
+
+  goals_dot_.resize(num_joints_);
+  goals_rt_dot_.resize(num_joints_);
+
+  goals_dot_dot_.resize(num_joints_);
+  goals_rt_dot_dot_.resize(num_joints_);
+
+  control_torque_.resize(num_joints_);
+
+  gravity_torque_ = new KDL::Vector[num_joints_+1];
+  gravity_torque_uncompensated_ = new KDL::Vector[num_joints_+1];
+
+  last_time_ = robot->hw_->current_time_;
+
+  return true;
+}
+
+void PR2ArmDynamicsController::setJointCmd(const std::vector<double> &j_values, const std::vector<double> &j_values_dot,  const std::vector<double> &j_values_dot_dot, const std::vector<std::string> & j_names)
+{
+  assert(j_values.size() == j_names.size());
+  for(uint i = 0; i < j_values.size(); ++i)
+  {
+    const std::string & name = j_names[i];
+    const int id = getJointControllerByName(name);
+    if(id >= 0)
+    {
+      goals_[id] = j_values[i];
+      goals_dot_[id] = j_values_dot[i];
+      goals_dot_dot_[id] = j_values_dot_dot[i];
+    }
+  }
+  goal_achieved_ = false;
+
+  arm_controller_lock_.lock();
+  refresh_rt_vals_ = true;
+  arm_controller_lock_.unlock();
+}
+
+
+void PR2ArmDynamicsController::getJointCmd(robot_msgs::JointCmd & cmd) const
+{
+  const unsigned int n = joint_effort_controllers_.size();
+  cmd.set_names_size(n);
+  for(unsigned int i=0; i<n; ++i)
+      cmd.names[i] = joint_effort_controllers_[i]->getJointName();
+
+  cmd.set_positions_vec(goals_);
+}
+
+
+controller::JointEffortController* PR2ArmDynamicsController::getJointEffortControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_effort_controllers_[i]->getJointName() == name)
+    {
+      return joint_effort_controllers_[i];
+    }
+  }
+    return NULL;
+}
+
+
+int PR2ArmDynamicsController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) num_joints_; i++)
+  {
+    if(joint_effort_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+
+void PR2ArmDynamicsController::update(void)
+{
+//  cout << "Updating dynamics controller " << std::endl;
+  double time = robot_->hw_->current_time_;
+
+  if(refresh_rt_vals_ && arm_controller_lock_.trylock())
+  {
+    for(unsigned int i=0; i < num_joints_; ++i)
+    {
+      goals_rt_[i] = goals_[i];
+      goals_rt_dot_[i] = goals_dot_[i];
+      goals_rt_dot_dot_[i] = goals_dot_dot_[i];
+    }
+    arm_controller_lock_.unlock();
+  }
+
+  computeControlTorque(time);
+
+  for(unsigned int i=0; i < num_joints_;++i)
+//  for(unsigned int i=5; i < num_joints_; ++i)
+  {
+   joint_effort_controllers_[i]->setCommand(control_torque_[i]);
+//    joint_effort_controllers_[i]->setCommand(0.0);
+  }
+
+  updateJointControllers();
+
+  for(unsigned int i=0; i < num_joints_; ++i)
+  {
+//    fprintf(stderr,"Effort: %d %f %f %f\n",i,control_torque_[i],gravity_torque_[i][2],joint_effort_controllers_[i]->joint_->commanded_effort_);
+  }
+}
+
+void PR2ArmDynamicsController::computeControlTorque(const double &time)
+{
+  KDL::JntArray kdl_q(num_joints_);
+  KDL::JntArray kdl_q_uncompensated(num_joints_);
+  KDL::JntArray kdl_q_dot(num_joints_);
+  KDL::JntArray kdl_q_dot_dot(num_joints_);
+  double error(0),command(0),actual(0),pid_torque(0);
+  int j_type;
+
+  for(unsigned int i=0; i < num_joints_; ++i)
+  {
+    kdl_q(i) = joint_effort_controllers_[i]->joint_state_->position_;
+    kdl_q_uncompensated(i) = kdl_q(i);
+  }
+  kdl_q_uncompensated(4) = 0.0;  
+  kdl_q_uncompensated(5) = 0.0;  
+  kdl_q_uncompensated(6) = 0.0;
+
+  arm_chain_->computeGravityTerms(kdl_q,gravity_torque_);
+  arm_chain_uncompensated_->computeGravityTerms(kdl_q_uncompensated,gravity_torque_uncompensated_);
+
+  for(unsigned int i=0; i < num_joints_; ++i)
+  {
+    command = goals_rt_[i];
+    actual = joint_effort_controllers_[i]->joint_state_->position_;
+    j_type = joint_effort_controllers_[i]->joint_state_->joint_->type_;
+    if(j_type == mechanism::JOINT_ROTARY)
+    {
+      if(!angles::shortest_angular_distance_with_limits(command, actual, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_min_, joint_effort_controllers_[i]->joint_state_->joint_->joint_limit_max_,error))
+        error = 0;
+    }
+    else if(j_type == mechanism::JOINT_CONTINUOUS)
+    {
+      error = angles::shortest_angular_distance(command, actual);
+    }
+    else
+    {
+      error = actual - command;
+    }
+    pid_torque = pid_controllers_[i].updatePid(error, time - last_time_);
+    if( i < 4)
+    {
+//      if(i == 2)
+//        control_torque_[i] = 0.0;
+//      else
+        //control_torque_[i] = gravity_torque_[i][2] + pid_torque;
+        control_torque_[i] = gravity_torque_[i][2] - gravity_torque_uncompensated_[i][2] + pid_torque;
+    }
+    else
+      control_torque_[i] = gravity_torque_[i][2] + pid_torque;
+//    fprintf(stderr,"%d:: %f, %f, %f, %f, %f, %f, %f\n",i,actual,command,error,pid_torque,gravity_torque_[i][2],control_torque_[i],time-last_time_);
+
+    static misc_utils::RealtimePublisher<std_msgs::String> p("/s", 1);
+    if (p.trylock()) {
+      char buf[1000];       
+      sprintf(buf, "Joint torques %d:: %s, %15.6lf %15.61f %15.61f %15.61f\n", i, joint_effort_controllers_[i]->joint_state_->joint_->name_.c_str(),control_torque_[i],gravity_torque_[i][2],gravity_torque_uncompensated_[i][2],pid_torque);
+      p.msg_.data = std::string(buf);
+      p.unlockAndPublish();
+    }
+
+
+
+  }
+
+
+  last_time_ = time;
+}
+
+
+void PR2ArmDynamicsController::updateJointControllers(void)
+{
+  for(unsigned int i=0;i<num_joints_;++i)
+    joint_effort_controllers_[i]->update();
+}
+
+
+void PR2ArmDynamicsController::checkForGoalAchieved_(void)
+{
+/*  goal_achieved_ = true;
+  for(unsigned int i=0;i<goals_rt_.size();++i)
+    goal_achieved_=goal_achieved_&&(error_margins_[i]<=0||std::abs(joint_effort_controllers_[i]->getMeasuredPosition()-goals_rt_[i])<error_margins_[i]);
+*/
+}
+
+
+//------ Arm controller node --------
+
+ROS_REGISTER_CONTROLLER(PR2ArmDynamicsControllerNode)
+
+PR2ArmDynamicsControllerNode::PR2ArmDynamicsControllerNode()
+  : Controller()
+{
+  ROS_INFO("PR2 arm gravity compensation controller node created");
+  c_ = new PR2ArmDynamicsController();
+}
+
+PR2ArmDynamicsControllerNode::~PR2ArmDynamicsControllerNode()
+{
+  delete c_;
+}
+
+void PR2ArmDynamicsControllerNode::update()
+{
+  c_->update();
+}
+
+bool PR2ArmDynamicsControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  ROS_INFO("LOADING PR2 ARM DYNAMICS CONTROLLER NODE");
+  ros::node * const node = ros::node::instance();
+  string prefix = config->Attribute("name");
+  std::cout<<"the prefix is "<<prefix<<std::endl;
+
+  // Parses controller configuration.
+  std::string kdl_chain_name="";
+  TiXmlElement *j = config->FirstChildElement("kinematics");
+  assert(j);
+  j = j->FirstChildElement("elem");
+  assert(j);
+  while(j!=NULL)
+  {
+    if(j->Attribute("key")==std::string("kdl_chain_name"))
+    {
+      kdl_chain_name = j->GetText();
+      break;
+    }
+    j = j->NextSiblingElement("elem");
+  }
+
+
+  // Parses subcontroller configuration
+  if(c_->initXml(robot, config))
+  {
+    node->advertise_service(prefix + "/set_command_array", &PR2ArmDynamicsControllerNode::setJointSrv, this);
+    node->advertise_service(prefix + "/get_command", &PR2ArmDynamicsControllerNode::getJointCmd, this);
+
+// Parses kinematics description
+    std::string pr2Contents;
+    std::string pr2_uncompensated;
+
+    node->get_param("robotdesc/pr2_uncompensated", pr2_uncompensated);
+    node->get_param("robotdesc/pr2", pr2Contents);
+
+    c_->pr2_kin_.loadString(pr2Contents.c_str());
+    c_->pr2_kin_uncompensated_.loadString(pr2_uncompensated.c_str());
+
+    c_->arm_chain_ = c_->pr2_kin_.getSerialChain(kdl_chain_name.c_str());
+    c_->arm_chain_uncompensated_ = c_->pr2_kin_uncompensated_.getSerialChain(kdl_chain_name.c_str());
+
+    fprintf(stderr,"Got arm chain %s\n",kdl_chain_name.c_str());
+    assert(c_->arm_chain_);
+    assert(c_->arm_chain_uncompensated_);
+    return true;
+  }
+  return false;
+}
+
+bool PR2ArmDynamicsControllerNode::setJointSrv(robot_srvs::SetJointCmd::request &req,
+                                   robot_srvs::SetJointCmd::response &resp)
+{
+  std::vector<double> pos;
+  std::vector<double> vel;
+  std::vector<double> acc;
+  std::vector<std::string> names;
+
+  req.set_positions_vec(pos);
+  req.set_velocity_vec(vel);
+  req.set_acc_vec(acc);
+  req.set_names_vec(names);
+
+  c_->setJointCmd(pos,vel,acc,names);
+  return true;
+}
+
+bool PR2ArmDynamicsControllerNode::getJointCmd(robot_srvs::GetJointCmd::request &req,
+                    robot_srvs::GetJointCmd::response &resp)
+{
+  robot_msgs::JointCmd cmd;
+  c_->getJointCmd(cmd);
+  resp.command = cmd;
+  return true;
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-01 00:19:46 UTC (rev 8792)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-01 01:10:24 UTC (rev 8793)
@@ -57,7 +57,7 @@
   /*********** Start moving the robot ************/
   pr2_mechanism_controllers::JointTraj cmd;
 
-  int num_points = 2;
+  int num_points = 1;
   int num_joints = 7;
 
   cmd.set_points_size(num_points);
@@ -65,6 +65,7 @@
   for(int i=0; i<num_points; i++)
     cmd.points[i].set_positions_size(num_joints);
 
+/*
   cmd.points[0].positions[0] = 0.0;
   cmd.points[0].positions[1] = 0.0;
   cmd.points[0].positions[2] = 0.0;
@@ -73,15 +74,16 @@
   cmd.points[0].positions[5] = 0.0;
   cmd.points[0].positions[6] = 0.0;
   cmd.points[0].time = 0.0;
+*/
 
-  cmd.points[1].positions[0] = 1.0;
-  cmd.points[1].positions[1] = 1.0;
-  cmd.points[1].positions[2] = 0.2;
-  cmd.points[1].positions[3] = -0.5;
-  cmd.points[1].positions[4] = 0.4;
-  cmd.points[1].positions[5] = 0.0;
-  cmd.points[1].positions[6] = 0.0;
-  cmd.points[1].time = 10.0;
+  cmd.points[0].positions[0] = 0.5;
+  cmd.points[0].positions[1] = 0.5;
+  cmd.points[0].positions[2] = 0.2;
+  cmd.points[0].positions[3] = 0.5;
+  cmd.points[0].positions[4] = 0.4;
+  cmd.points[0].positions[5] = 0.0;
+  cmd.points[0].positions[6] = 0.0;
+  cmd.points[0].time = 10.0;
 
   node->advertise<pr2_mechanism_controllers::JointTraj>("arm_trajectory_command",1);
   sleep(1);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9044
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9044&view=rev
Author:   sachinchitta
Date:     2009-01-08 09:09:50 +0000 (Thu, 08 Jan 2009)

Log Message:
-----------
services added for controller

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 08:26:51 UTC (rev 9043)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 09:09:50 UTC (rev 9044)
@@ -41,6 +41,7 @@
 #include <robot_mechanism_controllers/joint_position_controller.h>
 #include <robot_mechanism_controllers/joint_velocity_controller.h>
 #include <robot_mechanism_controllers/joint_effort_controller.h>
+#include <robot_mechanism_controllers/joint_pd_controller.h>
 
 // Services
 #include <pr2_mechanism_controllers/SetJointPosCmd.h>
@@ -58,6 +59,9 @@
 #include <pr2_mechanism_controllers/JointTraj.h>
 #include <pr2_mechanism_controllers/JointTrajPoint.h>
 
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+
 //Kinematics
 #include <robot_kinematics/robot_kinematics.h>
 #include <trajectory/trajectory.h>
@@ -105,11 +109,11 @@
 
     ros::thread::mutex arm_controller_lock_;
 
-    controller::JointPositionController* getJointControllerByName(std::string name);
+    controller::JointPDController* getJointControllerByName(std::string name);
 
     private:
 
-    std::vector<JointPositionController *> joint_position_controllers_;
+    std::vector<JointPDController *> joint_pd_controllers_;
 
     trajectory::Trajectory *joint_trajectory_;
 
@@ -117,6 +121,8 @@
 
     std::vector<double> joint_cmd_rt_;
 
+    std::vector<double> joint_cmd_dot_rt_;
+
     mechanism::Robot* robot_;
 
     void updateJointControllers(void);
@@ -128,6 +134,12 @@
 
     double trajectory_start_time_;
 
+    double trajectory_end_time_;
+
+    double current_time_;
+
+    bool trajectory_done_;
+
     int dimension_;
 
     friend class ArmTrajectoryControllerNode;
@@ -172,7 +184,18 @@
     bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::request &req,
                         pr2_mechanism_controllers::GetJointPosCmd::response &resp);
 
+    bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::request &req,
+                         pr2_mechanism_controllers::TrajectoryStart::response &resp);
 
+    bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
+                           pr2_mechanism_controllers::TrajectoryQuery::response &resp);
+
+    void deleteTrajectoryFromQueue(int id);
+
+    void addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id);
+
+    int createTrajectory(const pr2_mechanism_controllers::JointTraj new_traj,trajectory::Trajectory &return_trajectory);
+
     private:
 
     /*!
@@ -211,8 +234,18 @@
      */
     void CmdTrajectoryReceived();
 
+    int trajectory_id_;
+
+    std::vector<pr2_mechanism_controllers::JointTraj> joint_trajectory_vector_;
+
+    std::vector<int> joint_trajectory_id_;
+
+    void setTrajectoryCmdFromMsg(pr2_mechanism_controllers::JointTraj traj_msg);
+
+    int request_trajectory_id_;
+
+    int current_trajectory_id_;
+
   };
 
 }
-
-

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 08:26:51 UTC (rev 9043)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 09:09:50 UTC (rev 9044)
@@ -58,8 +58,8 @@
 ArmTrajectoryController::~ArmTrajectoryController()
 {
   // Assumes the joint controllers are owned by this class.
-  for(unsigned int i=0; i<joint_position_controllers_.size();++i)
-    delete joint_position_controllers_[i];
+  for(unsigned int i=0; i<joint_pd_controllers_.size();++i)
+    delete joint_pd_controllers_[i];
 }
 
 bool ArmTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
@@ -75,18 +75,18 @@
   TiXmlElement *elt = config->FirstChildElement("controller");
   while (elt)
   {
-    JointPositionController * jpc = new JointPositionController();
+    JointPDController * jpc = new JointPDController();
 //    std::cout<<elt->Attribute("type")<<elt->Attribute("name")<<std::endl;
     ROS_INFO("Joint Position Controller: %s , %s",(elt->Attribute("type")),(elt->Attribute("name")));
-    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPositionController"));
+    assert(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPDController"));
 
-    joint_position_controllers_.push_back(jpc);
+    joint_pd_controllers_.push_back(jpc);
     if(!jpc->initXml(robot, elt))
       return false;
 
     joint = (robot->getJointState(jpc->getJointName()))->joint_;
     if(joint)
-      joint_velocity_limits.push_back(joint->velocity_limit_/1.0);
+      joint_velocity_limits.push_back(joint->velocity_limit_/4.0);
 
     elt = elt->NextSiblingElement("controller");
   }
@@ -102,20 +102,21 @@
     trajectory_type = std::string(elt->Attribute("interpolation"));
     ROS_INFO("ArmTrajectoryController:: interpolation type:: %s",trajectory_type.c_str());    
   }
-  joint_cmd_rt_.resize(joint_position_controllers_.size()); 
+  joint_cmd_rt_.resize(joint_pd_controllers_.size()); 
+  joint_cmd_dot_rt_.resize(joint_pd_controllers_.size()); 
 
-  joint_trajectory_ = new trajectory::Trajectory((int) joint_position_controllers_.size());
+  joint_trajectory_ = new trajectory::Trajectory((int) joint_pd_controllers_.size());
 
   joint_trajectory_->setMaxRates(joint_velocity_limits);
   joint_trajectory_->setInterpolationMethod(trajectory_type);
 
-  trajectory_point_.setDimension((int) joint_position_controllers_.size());
-  dimension_ = (int) joint_position_controllers_.size();
+  trajectory_point_.setDimension((int) joint_pd_controllers_.size());
+  dimension_ = (int) joint_pd_controllers_.size();
 
 
 // Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
   for(int j=0; j < dimension_; j++)
-    trajectory_point_.q_[j] = joint_position_controllers_[j]->joint_state_->position_;
+    trajectory_point_.q_[j] = joint_pd_controllers_[j]->joint_state_->position_;
   trajectory_point_.time_ = 0.0;
   trajectory_points_vector.push_back(trajectory_point_);
 
@@ -154,13 +155,13 @@
 {
 }
 
-controller::JointPositionController* ArmTrajectoryController::getJointControllerByName(std::string name)
+controller::JointPDController* ArmTrajectoryController::getJointControllerByName(std::string name)
 {
-  for(int i=0; i< (int) joint_position_controllers_.size(); i++)
+  for(int i=0; i< (int) joint_pd_controllers_.size(); i++)
   {
-    if(joint_position_controllers_[i]->getJointName() == name)
+    if(joint_pd_controllers_[i]->getJointName() == name)
     {
-      return joint_position_controllers_[i];
+      return joint_pd_controllers_[i];
     }
   }
     return NULL;
@@ -168,9 +169,9 @@
 
 int ArmTrajectoryController::getJointControllerPosByName(std::string name)
 {
-  for(int i=0; i< (int) joint_position_controllers_.size(); i++)
+  for(int i=0; i< (int) joint_pd_controllers_.size(); i++)
   {
-    if(joint_position_controllers_[i]->getJointName() == name)
+    if(joint_pd_controllers_[i]->getJointName() == name)
     {
       return i;
     }
@@ -183,10 +184,15 @@
   double start_time = now();
 
   double sample_time(0.0);
+
+  current_time_ = robot_->hw_->current_time_;
+
   if(refresh_rt_vals_)
   {
     trajectory_start_time_ = robot_->hw_->current_time_;
+    trajectory_end_time_ = joint_trajectory_->getTotalTime()+trajectory_start_time_;
     refresh_rt_vals_ = false;
+    trajectory_done_ = false;
   }
   if(arm_controller_lock_.trylock())
   {
@@ -197,14 +203,20 @@
     for(unsigned int i=0; i < joint_cmd_rt_.size(); ++i)
     {
       joint_cmd_rt_[i] = trajectory_point_.q_[i];
+      joint_cmd_dot_rt_[i] = trajectory_point_.qdot_[i];
 //      cout << " " << joint_cmd_rt_[i];
     }
 //    cout << endl;
     arm_controller_lock_.unlock();
+
+    if(robot_->hw_->current_time_ >= trajectory_end_time_ && trajectory_done_ == false)
+    {
+      trajectory_done_= true;
+    }
   }
 
-  for(unsigned int i=0;i<joint_position_controllers_.size();++i)
-    joint_position_controllers_[i]->setCommand(joint_cmd_rt_[i]);
+  for(unsigned int i=0;i<joint_pd_controllers_.size();++i)
+    joint_pd_controllers_[i]->setCommand(joint_cmd_rt_[i],joint_cmd_dot_rt_[i]);
 
   updateJointControllers();
 
@@ -221,8 +233,8 @@
 
 void ArmTrajectoryController::updateJointControllers(void)
 {
-  for(unsigned int i=0;i<joint_position_controllers_.size();++i)
-    joint_position_controllers_[i]->update();
+  for(unsigned int i=0;i<joint_pd_controllers_.size();++i)
+    joint_pd_controllers_[i]->update();
 }
 
 //------ Arm controller node --------
@@ -230,7 +242,7 @@
 ROS_REGISTER_CONTROLLER(ArmTrajectoryControllerNode)
 
 ArmTrajectoryControllerNode::ArmTrajectoryControllerNode()
-  : Controller(), node_(ros::node::instance())
+  : Controller(), node_(ros::node::instance()), request_trajectory_id_(1), current_trajectory_id_(0)
 {
   std::cout<<"Controller node created"<<endl;
   c_ = new ArmTrajectoryController();
@@ -243,17 +255,34 @@
   node_->unadvertise_service(service_prefix_ + "/get_command");
   node_->unadvertise_service(service_prefix_ + "/set_target");
   */
+  node_->unadvertise_service("set_arm_traj_srv");
+
    if(topic_name_ptr_ && topic_name_.c_str())
   {
     std::cout << "unsub arm controller" << topic_name_ << std::endl;
-    node_->unsubscribe(topic_name_, &ArmTrajectoryControllerNode::CmdTrajectoryReceived, this);
+    node_->unsubscribe(topic_name_);
   }
-
   delete c_;
 }
 
 void ArmTrajectoryControllerNode::update()
 {
+  if(c_->trajectory_done_)
+  {
+    if(joint_trajectory_vector_.size() > 0)
+    {
+      if(current_trajectory_id_ == joint_trajectory_id_.front())
+      {
+        joint_trajectory_vector_.erase(joint_trajectory_vector_.begin());
+        joint_trajectory_id_.erase(joint_trajectory_id_.begin());
+      }
+      if(joint_trajectory_vector_.size() > 0)
+      {
+        setTrajectoryCmdFromMsg(joint_trajectory_vector_.front()); 
+        current_trajectory_id_ = joint_trajectory_id_.front();
+      }
+    }
+  }
   c_->update();
 }
 
@@ -270,6 +299,9 @@
     node_->advertise_service(service_prefix_ + "/get_command", &ArmTrajectoryControllerNode::getJointPosCmd, this);
     node_->advertise_service(service_prefix_ + "/set_target", &ArmTrajectoryControllerNode::setJointPosTarget, this);
 */
+
+   node_->advertise_service("set_arm_traj_srv", &ArmTrajectoryControllerNode::setJointTrajSrv, this);
+   
     topic_name_ptr_ = config->FirstChildElement("listen_topic");
     if(topic_name_ptr_)
     {
@@ -289,26 +321,25 @@
   return false;
 }
 
-void ArmTrajectoryControllerNode::CmdTrajectoryReceived()
+void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(pr2_mechanism_controllers::JointTraj traj_msg)
 {
   std::vector<trajectory::Trajectory::TPoint> tp;
+  tp.resize((int)traj_msg.get_points_size()+1);
 
-  this->ros_lock_.lock();
-  tp.resize((int)traj_msg_.get_points_size()+1);
-
   //set first point in trajectory to current position of the arm
     tp[0].setDimension((int) c_->dimension_);
+
   for(int j=0; j < c_->dimension_; j++)
   {
-    tp[0].q_[j] = c_->joint_position_controllers_[j]->joint_state_->position_;
+    tp[0].q_[j] = c_->joint_pd_controllers_[j]->joint_state_->position_;
     tp[0].time_ = 0.0;
   }
 
-  if(traj_msg_.get_points_size() > 0)
+  if(traj_msg.get_points_size() > 0)
   {
-     if((int) traj_msg_.points[0].get_positions_size() != (int) c_->dimension_)
+     if((int) traj_msg.points[0].get_positions_size() != (int) c_->dimension_)
      {
-       ROS_WARN("Dimension of input trajectory = %d does not match number of controlled joints = %d",(int) traj_msg_.points[0].get_positions_size(), (int) c_->dimension_);
+       ROS_WARN("Dimension of input trajectory = %d does not match number of controlled joints = %d",(int) traj_msg.points[0].get_positions_size(), (int) c_->dimension_);
         return;
      }
   }
@@ -318,17 +349,23 @@
      return;
   }
 
-  for(int i=0; i < (int) traj_msg_.get_points_size(); i++)
+  for(int i=0; i < (int) traj_msg.get_points_size(); i++)
   {
      tp[i+1].setDimension((int) c_->dimension_);
      for(int j=0; j < (int) c_->dimension_; j++)
      {
-        tp[i+1].q_[j] = traj_msg_.points[i].positions[j];
-        tp[i+1].time_ = traj_msg_.points[i].time;
+        tp[i+1].q_[j] = traj_msg.points[i].positions[j];
+        tp[i+1].time_ = traj_msg.points[i].time;
      }
   }
+  this->c_->setTrajectoryCmd(tp);
+}
 
-  this->c_->setTrajectoryCmd(tp);
+void ArmTrajectoryControllerNode::CmdTrajectoryReceived()
+{
+
+  this->ros_lock_.lock();
+  setTrajectoryCmdFromMsg(traj_msg_);
   this->ros_lock_.unlock();
 }
 
@@ -341,3 +378,122 @@
   resp.command = cmd;
   return true;
 }
+
+
+bool ArmTrajectoryControllerNode::setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::request &req,
+                    pr2_mechanism_controllers::TrajectoryStart::response &resp)
+{
+  addTrajectoryToQueue(req.traj, request_trajectory_id_);
+  resp.trajectoryid = request_trajectory_id_;
+
+  if(req.requesttiming)
+  {
+    trajectory::Trajectory tmp(c_->dimension_);
+    createTrajectory(req.traj,tmp);
+    std::vector<double> timestamps;
+
+    resp.set_timestamps_size((int)req.traj.get_points_size());
+    timestamps.resize((int)req.traj.get_points_size());
+
+    tmp.getTimeStamps(timestamps);
+
+    for(int i=0; i < (int) req.traj.get_points_size(); i++)
+    {
+      resp.timestamps[i] = timestamps[i];
+    }
+  }
+
+  request_trajectory_id_++;
+  return true;
+}
+
+bool ArmTrajectoryControllerNode::queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
+                                                    pr2_mechanism_controllers::TrajectoryQuery::response &resp)
+{
+  if(current_trajectory_id_ == (int) req.trajectoryid)
+  {
+    if(c_->current_time_ <= c_->trajectory_end_time_)
+    {
+      resp.trajectorytime = c_->current_time_-c_->trajectory_start_time_;
+      resp.done = 0;
+    }
+    else
+    {
+      resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
+      resp.done = 1;
+    }
+  }
+  else if(current_trajectory_id_ >= (int)req.trajectoryid)
+  {
+    resp.trajectorytime = 0;
+    resp.done = 1;
+  }
+  else
+  {
+    resp.trajectorytime = 0;
+    resp.done = 2;
+  }
+  resp.set_jointnames_size(c_->dimension_);
+  for(int i=0; i < c_->dimension_; i++)
+  {
+    resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
+    
+  }
+  return true;
+}
+
+int ArmTrajectoryControllerNode::createTrajectory(const pr2_mechanism_controllers::JointTraj new_traj,trajectory::Trajectory &return_trajectory)
+{
+  std::vector<trajectory::Trajectory::TPoint> tp;
+  tp.resize((int)new_traj.get_points_size());
+
+  if(new_traj.get_points_size() > 0)
+  {
+     if((int) new_traj.points[0].get_positions_size() != (int) c_->dimension_)
+     {
+       ROS_WARN("Dimension of input trajectory = %d does not match number of controlled joints = %d",(int) new_traj.points[0].get_positions_size(), (int) c_->dimension_);
+        return -1;
+     }
+  }
+  else
+  {
+    ROS_WARN("Trajectory message has no way points");
+     return -1;
+  }
+
+  for(int i=0; i < (int) new_traj.get_points_size(); i++)
+  {
+     tp[i].setDimension((int) c_->dimension_);
+     for(int j=0; j < (int) c_->dimension_; j++)
+     {
+        tp[i].q_[j] = traj_msg_.points[i].positions[j];
+        tp[i].time_ = traj_msg_.points[i].time;
+     }
+  }
+  if(!return_trajectory.setTrajectory(tp))
+  {
+    ROS_WARN("Trajectory not set correctly");
+    return -1;
+  }
+  return 1;
+}
+
+void ArmTrajectoryControllerNode::addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id)
+{
+  joint_trajectory_vector_.push_back(new_traj);
+  joint_trajectory_id_.push_back(id);
+}
+
+void ArmTrajectoryControllerNode::deleteTrajectoryFromQueue(int id)
+{
+// do a linear search
+  for(int i = 0; i < (int) joint_trajectory_vector_.size(); i++)
+  {
+    if(joint_trajectory_id_[i] == id)
+    {
+      joint_trajectory_vector_.erase(joint_trajectory_vector_.begin()+i);
+      joint_trajectory_id_.erase(joint_trajectory_id_.begin()+i);
+      break;
+    }
+  }
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-08 08:26:51 UTC (rev 9043)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-08 09:09:50 UTC (rev 9044)
@@ -57,7 +57,7 @@
   /*********** Start moving the robot ************/
   pr2_mechanism_controllers::JointTraj cmd;
 
-  int num_points = 1;
+  int num_points = 3;
   int num_joints = 7;
 
   cmd.set_points_size(num_points);
@@ -79,12 +79,30 @@
   cmd.points[0].positions[0] = 0.5;
   cmd.points[0].positions[1] = 0.5;
   cmd.points[0].positions[2] = 0.2;
-  cmd.points[0].positions[3] = 0.5;
+  cmd.points[0].positions[3] = -0.5;
   cmd.points[0].positions[4] = 0.4;
   cmd.points[0].positions[5] = 0.0;
   cmd.points[0].positions[6] = 0.0;
-  cmd.points[0].time = 10.0;
+  cmd.points[0].time = 0.0;
 
+  cmd.points[1].positions[0] = 0.0;
+  cmd.points[1].positions[1] = 0.0;
+  cmd.points[1].positions[2] = 0.0;
+  cmd.points[1].positions[3] = 0.0;
+  cmd.points[1].positions[4] = 0.0;
+  cmd.points[1].positions[5] = 0.0;
+  cmd.points[1].positions[6] = 0.0;
+  cmd.points[1].time = 0.0;
+
+  cmd.points[2].positions[0] = -0.5;
+  cmd.points[2].positions[1] = 0.3;
+  cmd.points[2].positions[2] = 0.2;
+  cmd.points[2].positions[3] = -1.0;
+  cmd.points[2].positions[4] = -0.4;
+  cmd.points[2].positions[5] = 0.0;
+  cmd.points[2].positions[6] = 0.0;
+  cmd.points[2].time = 0.0;
+
   node->advertise<pr2_mechanism_controllers::JointTraj>("arm_trajectory_command",1);
   sleep(1);
   node->publish("arm_trajectory_command",cmd);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9050
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9050&view=rev
Author:   sachinchitta
Date:     2009-01-08 11:59:33 +0000 (Thu, 08 Jan 2009)

Log Message:
-----------
Added trajectory services to the controllers and a sample test file.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 11:52:22 UTC (rev 9049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 11:59:33 UTC (rev 9050)
@@ -142,6 +142,10 @@
 
     int dimension_;
 
+    std::vector<double> joint_velocity_limits_;
+
+    std::string trajectory_type_;
+
     friend class ArmTrajectoryControllerNode;
   };
 
@@ -194,7 +198,7 @@
 
     void addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id);
 
-    int createTrajectory(const pr2_mechanism_controllers::JointTraj new_traj,trajectory::Trajectory &return_trajectory);
+    int createTrajectory(const pr2_mechanism_controllers::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
 
     private:
 
@@ -246,6 +250,9 @@
 
     int current_trajectory_id_;
 
+    std::map<int,int> joint_trajectory_status_;
+
+    std::map<int,double>joint_trajectory_time_;
   };
 
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 11:52:22 UTC (rev 9049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 11:59:33 UTC (rev 9050)
@@ -51,7 +51,7 @@
 }
 
 ArmTrajectoryController::ArmTrajectoryController() :
-  refresh_rt_vals_(false)
+  refresh_rt_vals_(false),trajectory_type_("linear")
 {
 }
 
@@ -68,9 +68,9 @@
 
   robot_ = robot->model_;
   mechanism::Joint *joint;
-  std::vector<double> joint_velocity_limits;
+//  std::vector<double> joint_velocity_limits;
   std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
-  std::string trajectory_type = "linear";
+//  std::string trajectory_type = "linear";
 
   TiXmlElement *elt = config->FirstChildElement("controller");
   while (elt)
@@ -86,7 +86,7 @@
 
     joint = (robot->getJointState(jpc->getJointName()))->joint_;
     if(joint)
-      joint_velocity_limits.push_back(joint->velocity_limit_/4.0);
+      joint_velocity_limits_.push_back(joint->velocity_limit_/4.0);
 
     elt = elt->NextSiblingElement("controller");
   }
@@ -99,16 +99,16 @@
     ROS_WARN("No trajectory information in xml file. ");
   else
   {
-    trajectory_type = std::string(elt->Attribute("interpolation"));
-    ROS_INFO("ArmTrajectoryController:: interpolation type:: %s",trajectory_type.c_str());    
+    trajectory_type_ = std::string(elt->Attribute("interpolation"));
+    ROS_INFO("ArmTrajectoryController:: interpolation type:: %s",trajectory_type_.c_str());    
   }
   joint_cmd_rt_.resize(joint_pd_controllers_.size()); 
   joint_cmd_dot_rt_.resize(joint_pd_controllers_.size()); 
 
   joint_trajectory_ = new trajectory::Trajectory((int) joint_pd_controllers_.size());
 
-  joint_trajectory_->setMaxRates(joint_velocity_limits);
-  joint_trajectory_->setInterpolationMethod(trajectory_type);
+  joint_trajectory_->setMaxRates(joint_velocity_limits_);
+  joint_trajectory_->setInterpolationMethod(trajectory_type_);
 
   trajectory_point_.setDimension((int) joint_pd_controllers_.size());
   dimension_ = (int) joint_pd_controllers_.size();
@@ -274,6 +274,8 @@
     {
       if(current_trajectory_id_ == joint_trajectory_id_.front())
       {
+        joint_trajectory_status_[current_trajectory_id_] = 1;
+        joint_trajectory_time_[current_trajectory_id_] = c_->trajectory_end_time_ - c_->trajectory_start_time_;
         joint_trajectory_vector_.erase(joint_trajectory_vector_.begin());
         joint_trajectory_id_.erase(joint_trajectory_id_.begin());
       }
@@ -281,6 +283,7 @@
       {
         setTrajectoryCmdFromMsg(joint_trajectory_vector_.front()); 
         current_trajectory_id_ = joint_trajectory_id_.front();
+        joint_trajectory_status_[current_trajectory_id_] = 0;
       }
     }
   }
@@ -393,7 +396,6 @@
     trajectory::Trajectory tmp(c_->dimension_);
     createTrajectory(req.traj,tmp);
     std::vector<double> timestamps;
-
     resp.set_timestamps_size((int)req.traj.get_points_size());
     timestamps.resize((int)req.traj.get_points_size());
 
@@ -412,27 +414,26 @@
 bool ArmTrajectoryControllerNode::queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
                                                     pr2_mechanism_controllers::TrajectoryQuery::response &resp)
 {
-  if(current_trajectory_id_ == (int) req.trajectoryid)
+  std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
+  if(it == joint_trajectory_status_.end())
+    return false;
+
+  resp.done = it->second;
+
+  if(current_trajectory_id_ == (int)req.trajectoryid)
   {
-    if(c_->current_time_ <= c_->trajectory_end_time_)
-    {
-      resp.trajectorytime = c_->current_time_-c_->trajectory_start_time_;
-      resp.done = 0;
-    }
+    if((int) resp.done == 1)
+      resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
     else
-    {
-      resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
-      resp.done = 1;
-    }
+      resp.trajectorytime = c_->current_time_ - c_->trajectory_start_time_;
   }
-  else if(current_trajectory_id_ >= (int)req.trajectoryid){
-    resp.trajectorytime = 0;
-    resp.done = 1;
-  }
   else
   {
-    resp.trajectorytime = 0;
-    resp.done = 2;
+    std::map<int, double>::const_iterator it_time = joint_trajectory_time_.find((int)req.trajectoryid);
+    if(it_time == joint_trajectory_time_.end())
+      return false;
+
+    resp.trajectorytime = it_time->second;
   }
   resp.set_jointnames_size(c_->dimension_);
   for(int i=0; i < c_->dimension_; i++)
@@ -443,10 +444,9 @@
   return true;
 }
 
-int ArmTrajectoryControllerNode::createTrajectory(const pr2_mechanism_controllers::JointTraj new_traj,trajectory::Trajectory &return_trajectory)
+int ArmTrajectoryControllerNode::createTrajectory(const pr2_mechanism_controllers::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
 {
   std::vector<trajectory::Trajectory::TPoint> tp;
-  tp.resize((int)new_traj.get_points_size());
 
   if(new_traj.get_points_size() > 0)
   {
@@ -462,15 +462,25 @@
      return -1;
   }
 
+  tp.resize((int)new_traj.get_points_size());
+  int size = (int)new_traj.get_points_size();
+
+//  ROS_INFO("size: %d",size);
+
   for(int i=0; i < (int) new_traj.get_points_size(); i++)
   {
      tp[i].setDimension((int) c_->dimension_);
+//     ROS_INFO("size vec: %d",tp[i].q_.size());
+//     int dim = tp[i].q_.size();
      for(int j=0; j < (int) c_->dimension_; j++)
      {
-        tp[i].q_[j] = traj_msg_.points[i].positions[j];
-        tp[i].time_ = traj_msg_.points[i].time;
+        tp[i].q_[j] = new_traj.points[i].positions[j];
+        tp[i].time_ = new_traj.points[i].time;
      }
   }
+  return_trajectory.setMaxRates(c_->joint_velocity_limits_);
+  return_trajectory.setInterpolationMethod(c_->trajectory_type_);
+
   if(!return_trajectory.setTrajectory(tp))
   {
     ROS_WARN("Trajectory not set correctly");
@@ -483,6 +493,8 @@
 {
   joint_trajectory_vector_.push_back(new_traj);
   joint_trajectory_id_.push_back(id);
+  joint_trajectory_status_[id] = 2;
+  joint_trajectory_time_[id] = 0.0;
 }
 
 void ArmTrajectoryControllerNode::deleteTrajectoryFromQueue(int id)
@@ -494,7 +506,9 @@
     {
       joint_trajectory_vector_.erase(joint_trajectory_vector_.begin()+i);
       joint_trajectory_id_.erase(joint_trajectory_id_.begin()+i);
+      joint_trajectory_status_[id] = 3;
       break;
     }
   }
 }
+

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp	2009-01-08 11:52:22 UTC (rev 9049)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp	2009-01-08 11:59:33 UTC (rev 9050)
@@ -64,7 +64,7 @@
   int num_joints = 7;
 
   req.traj.set_points_size(num_points);
-  req.requesttiming = 0;
+  req.requesttiming = 1;
 
   for(int i=0; i<num_points; i++)
     req.traj.points[i].set_positions_size(num_joints);
@@ -109,4 +109,10 @@
   {
     ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
   }
+  sleep(10);
+  if(ros::service::call("query_arm_traj_srv", req_q, res_q))  
+  {
+    ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
+  }
+
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9051
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9051&view=rev
Author:   sachinchitta
Date:     2009-01-08 12:21:14 +0000 (Thu, 08 Jan 2009)

Log Message:
-----------
added parameter for scaling velocity limits

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 11:59:33 UTC (rev 9050)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-08 12:21:14 UTC (rev 9051)
@@ -146,6 +146,8 @@
 
     std::string trajectory_type_;
 
+    double velocity_scaling_factor_;
+
     friend class ArmTrajectoryControllerNode;
   };
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 11:59:33 UTC (rev 9050)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-08 12:21:14 UTC (rev 9051)
@@ -86,7 +86,7 @@
 
     joint = (robot->getJointState(jpc->getJointName()))->joint_;
     if(joint)
-      joint_velocity_limits_.push_back(joint->velocity_limit_/4.0);
+      joint_velocity_limits_.push_back(joint->velocity_limit_*velocity_scaling_factor_);
 
     elt = elt->NextSiblingElement("controller");
   }
@@ -296,6 +296,11 @@
   service_prefix_ = config->Attribute("name");
   ROS_INFO("The service_prefix_ is %s",service_prefix_.c_str());
 
+  double scale;
+  node_->param<double>(service_prefix_ + "/velocity_scaling_factor",scale,0.25);
+
+  c_->velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
+
   if(c_->initXml(robot, config))  // Parses subcontroller configuration
   {
 /*    node_->advertise_service(service_prefix_ + "/set_command", &ArmTrajectoryControllerNode::setJointPosHeadless, this);
@@ -463,15 +468,10 @@
   }
 
   tp.resize((int)new_traj.get_points_size());
-  int size = (int)new_traj.get_points_size();
 
-//  ROS_INFO("size: %d",size);
-
   for(int i=0; i < (int) new_traj.get_points_size(); i++)
   {
      tp[i].setDimension((int) c_->dimension_);
-//     ROS_INFO("size vec: %d",tp[i].q_.size());
-//     int dim = tp[i].q_.size();
      for(int j=0; j < (int) c_->dimension_; j++)
      {
         tp[i].q_[j] = new_traj.points[i].positions[j];


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9087
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9087&view=rev
Author:   pantofaru
Date:     2009-01-09 00:09:51 +0000 (Fri, 09 Jan 2009)

Log Message:
-----------
Fixed the tf transform bug

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-01-09 00:04:50 UTC (rev 9086)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-01-09 00:09:51 UTC (rev 9087)
@@ -61,6 +61,7 @@
 #include <misc_utils/realtime_publisher.h>
 
 #include <tf/tfMessage.h>
+#include <tf/tf.h>
 
 #include <pthread.h>
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-01-09 00:04:50 UTC (rev 9086)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-01-09 00:09:51 UTC (rev 9087)
@@ -49,6 +49,7 @@
 using namespace control_toolbox;
 using namespace libTF;
 using namespace NEWMAT;
+using namespace tf;
 
 ROS_REGISTER_CONTROLLER(BaseController)
 
@@ -875,11 +876,18 @@
       out.transform.translation.x = -x*cos(yaw) - y*sin(yaw);
       out.transform.translation.y = +x*sin(yaw) - y*cos(yaw);
       out.transform.translation.z = 0;
-      out.transform.rotation.x = 0;
-      out.transform.rotation.y = 0;
+      tf::Quaternion quat_trans = tf::Quaternion(-yaw,0.0,0.0);
+
+      out.transform.rotation.x = quat_trans.x();
+      out.transform.rotation.y = quat_trans.y();
+      out.transform.rotation.z = quat_trans.z();
+      out.transform.rotation.w = quat_trans.w();
+
+      //      out.transform.rotation.x = 0;
+      //      out.transform.rotation.y = 0;
       double angle = angles::normalize_angle(-yaw);
-      out.transform.rotation.z = sqrt(1/(1 + pow(angle, 2)));
-      out.transform.rotation.w = sqrt(pow(angle, 2) / (1 + pow(angle, 2)));
+      //      out.transform.rotation.z = sqrt(1/(1 + pow(angle, 2)));
+      //      out.transform.rotation.w = sqrt(pow(angle, 2) / (1 + pow(angle, 2)));
       //      out.pitch = 0;
       //out.yaw = 
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9123
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9123&view=rev
Author:   sachinchitta
Date:     2009-01-09 08:37:52 +0000 (Fri, 09 Jan 2009)

Log Message:
-----------
added service prefix to all services

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-09 08:14:41 UTC (rev 9122)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-09 08:37:52 UTC (rev 9123)
@@ -255,8 +255,8 @@
   node_->unadvertise_service(service_prefix_ + "/get_command");
   node_->unadvertise_service(service_prefix_ + "/set_target");
   */
-  node_->unadvertise_service("TrajectoryStart");
-  node_->unadvertise_service("TrajectoryQuery");
+  node_->unadvertise_service(service_prefix_ + "/TrajectoryStart");
+  node_->unadvertise_service(service_prefix_ + "/TrajectoryQuery");
 
    if(topic_name_ptr_ && topic_name_.c_str())
   {
@@ -309,8 +309,8 @@
     node_->advertise_service(service_prefix_ + "/set_target", &ArmTrajectoryControllerNode::setJointPosTarget, this);
 */
 
-      node_->advertise_service("TrajectoryStart", &ArmTrajectoryControllerNode::setJointTrajSrv, this);
-   node_->advertise_service("TrajectoryQuery", &ArmTrajectoryControllerNode::queryJointTrajSrv, this);
+   node_->advertise_service(service_prefix_ + "TrajectoryStart", &ArmTrajectoryControllerNode::setJointTrajSrv, this);
+   node_->advertise_service(service_prefix_ + "TrajectoryQuery", &ArmTrajectoryControllerNode::queryJointTrajSrv, this);
    
     topic_name_ptr_ = config->FirstChildElement("listen_topic");
     if(topic_name_ptr_)

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp	2009-01-09 08:14:41 UTC (rev 9122)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_service.cpp	2009-01-09 08:37:52 UTC (rev 9123)
@@ -105,7 +105,7 @@
 
   req_q.trajectoryid = atoi(argv[1]);
 
-  if(ros::service::call("TrajectoryQuery", req_q, res_q))  
+  if(ros::service::call("right_arm_controller/TrajectoryQuery", req_q, res_q))  
   {
     ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
   }
@@ -114,7 +114,7 @@
           ROS_INFO("service call failed");
       }
   sleep(4);
-  if(ros::service::call("TrajectoryQuery", req_q, res_q))  
+  if(ros::service::call("right_arm_controller/TrajectoryQuery", req_q, res_q))  
   {
     ROS_INFO("response:: %f, %d",res_q.trajectorytime,res_q.done);
   }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9145
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9145&view=rev
Author:   sachinchitta
Date:     2009-01-09 22:45:56 +0000 (Fri, 09 Jan 2009)

Log Message:
-----------
added trajectory cancel service, wait (with timeout) for reaching a goal

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-09 22:22:08 UTC (rev 9144)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-09 22:45:56 UTC (rev 9145)
@@ -72,6 +72,8 @@
 namespace controller
 {
 
+  #define GOAL_REACHED_THRESHOLD 0.01
+
 // The maximum number of joints expected in an arm.
   static const int MAX_ARM_JOINTS = 7;
 
@@ -127,6 +129,8 @@
 
     void updateJointControllers(void);
 
+    bool reachedGoalPosition(std::vector<double> joint_cmd);
+
     int getJointControllerPosByName(std::string name);
 
     // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
@@ -149,6 +153,10 @@
     double velocity_scaling_factor_;
 
     friend class ArmTrajectoryControllerNode;
+
+    double trajectory_wait_time_;
+
+    std::vector<double> goal_reached_threshold_;
   };
 
 /** @class ArmTrajectoryControllerNode
@@ -196,12 +204,19 @@
     bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
                            pr2_mechanism_controllers::TrajectoryQuery::response &resp);
 
+    bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
+                            pr2_mechanism_controllers::TrajectoryQuery::response &resp);
+
     void deleteTrajectoryFromQueue(int id);
 
     void addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id);
 
     int createTrajectory(const pr2_mechanism_controllers::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
 
+    void updateTrajectoryQueue(int last_trajectory_finish_status);
+
+    void getJointTrajectoryThresholds();
+
     private:
 
     /*!
@@ -255,6 +270,19 @@
     std::map<int,int> joint_trajectory_status_;
 
     std::map<int,double>joint_trajectory_time_;
+
+    enum JointTrajectoryStatus{
+      ACTIVE,
+      DONE,
+      QUEUED,
+      DELETED,
+      FAILED,
+      CANCELED,
+      NUM_STATUS
+    };
+
+    double trajectory_wait_timeout_;
+
   };
 
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-09 22:22:08 UTC (rev 9144)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-09 22:45:56 UTC (rev 9145)
@@ -51,7 +51,7 @@
 }
 
 ArmTrajectoryController::ArmTrajectoryController() :
-  refresh_rt_vals_(false),trajectory_type_("linear")
+  refresh_rt_vals_(false),trajectory_type_("linear"),trajectory_wait_time_(0.0)
 {
 }
 
@@ -121,7 +121,7 @@
   trajectory_points_vector.push_back(trajectory_point_);
 
   for(int i=0; i < dimension_; i++)
-     trajectory_point_.q_[i] = 0.0;
+     trajectory_point_.q_[i] = joint_pd_controllers_[i]->joint_state_->position_;
   trajectory_point_.time_ = 0.0;
   trajectory_points_vector.push_back(trajectory_point_);
 
@@ -190,6 +190,7 @@
   if(refresh_rt_vals_)
   {
     trajectory_start_time_ = robot_->hw_->current_time_;
+    trajectory_wait_time_ = 0.0;
     trajectory_end_time_ = joint_trajectory_->getTotalTime()+trajectory_start_time_;
     refresh_rt_vals_ = false;
     trajectory_done_ = false;
@@ -211,7 +212,12 @@
 
     if(robot_->hw_->current_time_ >= trajectory_end_time_ && trajectory_done_ == false)
     {
-      trajectory_done_= true;
+      trajectory_wait_time_ = robot_->hw_->current_time_ - trajectory_end_time_;
+      if(reachedGoalPosition(joint_cmd_rt_))
+      {
+        trajectory_wait_time_ = 0.0;
+        trajectory_done_= true;
+      }
     }
   }
 
@@ -231,6 +237,17 @@
   }
 }
 
+bool ArmTrajectoryController::reachedGoalPosition(std::vector<double> joint_cmd)
+{
+  bool return_val = true;
+  double error(0.0);
+  for(unsigned int i=0;i< (unsigned int) dimension_;++i){
+    error = fabs(joint_pd_controllers_[i]->joint_state_->position_ - joint_cmd[i]);
+    return_val = return_val && (error <= goal_reached_threshold_[i]);
+  }
+  return return_val;
+}
+
 void ArmTrajectoryController::updateJointControllers(void)
 {
   for(unsigned int i=0;i<joint_pd_controllers_.size();++i)
@@ -242,7 +259,7 @@
 ROS_REGISTER_CONTROLLER(ArmTrajectoryControllerNode)
 
 ArmTrajectoryControllerNode::ArmTrajectoryControllerNode()
-  : Controller(), node_(ros::node::instance()), request_trajectory_id_(1), current_trajectory_id_(0)
+  : Controller(), node_(ros::node::instance()), request_trajectory_id_(1), current_trajectory_id_(0), trajectory_wait_timeout_(0.0)
 {
   std::cout<<"Controller node created"<<endl;
   c_ = new ArmTrajectoryController();
@@ -270,26 +287,36 @@
 {
   if(c_->trajectory_done_)
   {
+    updateTrajectoryQueue( ArmTrajectoryControllerNode::DONE);
+  }
+  if(c_->trajectory_wait_time_ >= trajectory_wait_timeout_)
+  {
+    updateTrajectoryQueue( ArmTrajectoryControllerNode::FAILED);    
+  }
+  c_->update();
+}
+
+void ArmTrajectoryControllerNode::updateTrajectoryQueue(int last_trajectory_finish_status)
+{
+  if(joint_trajectory_vector_.size() > 0)
+  {
+    if(current_trajectory_id_ == joint_trajectory_id_.front())
+    {
+      joint_trajectory_status_[current_trajectory_id_] = last_trajectory_finish_status;
+      joint_trajectory_time_[current_trajectory_id_] = c_->trajectory_end_time_ - c_->trajectory_start_time_;
+      joint_trajectory_vector_.erase(joint_trajectory_vector_.begin());
+      joint_trajectory_id_.erase(joint_trajectory_id_.begin());
+    }
     if(joint_trajectory_vector_.size() > 0)
     {
-      if(current_trajectory_id_ == joint_trajectory_id_.front())
-      {
-        joint_trajectory_status_[current_trajectory_id_] = 1;
-        joint_trajectory_time_[current_trajectory_id_] = c_->trajectory_end_time_ - c_->trajectory_start_time_;
-        joint_trajectory_vector_.erase(joint_trajectory_vector_.begin());
-        joint_trajectory_id_.erase(joint_trajectory_id_.begin());
-      }
-      if(joint_trajectory_vector_.size() > 0)
-      {
-        setTrajectoryCmdFromMsg(joint_trajectory_vector_.front()); 
-        current_trajectory_id_ = joint_trajectory_id_.front();
-        joint_trajectory_status_[current_trajectory_id_] = 0;
-      }
+      setTrajectoryCmdFromMsg(joint_trajectory_vector_.front()); 
+      current_trajectory_id_ = joint_trajectory_id_.front();
+      joint_trajectory_status_[current_trajectory_id_] = ArmTrajectoryControllerNode::ACTIVE;
     }
   }
-  c_->update();
 }
 
+
 bool ArmTrajectoryControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
 {
   ROS_INFO("Loading ArmTrajectoryControllerNode.");
@@ -298,6 +325,7 @@
 
   double scale;
   node_->param<double>(service_prefix_ + "/velocity_scaling_factor",scale,0.25);
+  node_->param<double>(service_prefix_ + "/trajectory_wait_timeout",trajectory_wait_timeout_,10.0);
 
   c_->velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
 
@@ -324,6 +352,9 @@
       node_->subscribe(topic_name_, traj_msg_, &ArmTrajectoryControllerNode::CmdTrajectoryReceived, this, 1);
       ROS_INFO("Listening to topic: %s",topic_name_.c_str());
     }
+
+    getJointTrajectoryThresholds();
+
     ROS_INFO("Initialized controller");
     return true;
   }
@@ -331,6 +362,15 @@
   return false;
 }
 
+void ArmTrajectoryControllerNode::getJointTrajectoryThresholds()
+{
+  c_->goal_reached_threshold_.resize(c_->dimension_);
+  for(int i=0; i< c_->dimension_;i++)
+  {
+    node_->param<double>(service_prefix_ + c_->joint_pd_controllers_[i]->getJointName() + "/goal_reached_threshold",c_->goal_reached_threshold_[i],GOAL_REACHED_THRESHOLD);
+  }
+}
+
 void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(pr2_mechanism_controllers::JointTraj traj_msg)
 {
   std::vector<trajectory::Trajectory::TPoint> tp;
@@ -427,23 +467,23 @@
   }
 
   if(req.trajectoryid == 0)
-      {
-          resp.trajectorytime = 0;
-          if(joint_trajectory_vector_.size() == 0)
-              resp.done = 1;
-          else
-              resp.done = 0;
-          return true;
-      }
+  {
+    resp.trajectorytime = 0;
+    if(joint_trajectory_vector_.size() == 0)
+      resp.done = 1;
+    else
+      resp.done = 0;
+    return true;
+  }
 
   std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
   if(it == joint_trajectory_status_.end())
     return false;
   else
-      resp.done = it->second;
+    resp.done = it->second;
   
 
-if(current_trajectory_id_ == (int)req.trajectoryid)
+  if(current_trajectory_id_ == (int)req.trajectoryid)
   {
     if((int) resp.done == 1)
       resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
@@ -458,8 +498,42 @@
 
     resp.trajectorytime = it_time->second;
   }
+  return true;
+}
 
+bool ArmTrajectoryControllerNode::cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::request &req,
+                                                     pr2_mechanism_controllers::TrajectoryQuery::response &resp)
+{
+  int status = ArmTrajectoryControllerNode::NUM_STATUS;
 
+  std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
+  std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
+
+  if(it == joint_trajectory_status_.end())
+    return false;
+  else
+    status = it->second;
+
+  if(status == ArmTrajectoryControllerNode::QUEUED)
+  {
+    deleteTrajectoryFromQueue(req.trajectoryid);
+  }
+  else if(resp.done == ArmTrajectoryControllerNode::ACTIVE)
+  {    
+    updateTrajectoryQueue(CANCELED);
+    // Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
+    for(int j=0; j < c_->dimension_; j++)
+      c_->trajectory_point_.q_[j] = c_->joint_pd_controllers_[j]->joint_state_->position_;
+    c_->trajectory_point_.time_ = 0.0;
+    trajectory_points_vector.push_back(c_->trajectory_point_);
+
+    for(int i=0; i < c_->dimension_; i++)
+      c_->trajectory_point_.q_[i] =  c_->joint_pd_controllers_[i]->joint_state_->position_;
+    c_->trajectory_point_.time_ = 0.0;
+    trajectory_points_vector.push_back(c_->trajectory_point_);
+    if(!c_->joint_trajectory_->setTrajectory(trajectory_points_vector))
+      ROS_WARN("Trajectory not set correctly");
+  }
   return true;
 }
 
@@ -507,7 +581,7 @@
 {
   joint_trajectory_vector_.push_back(new_traj);
   joint_trajectory_id_.push_back(id);
-  joint_trajectory_status_[id] = 2;
+  joint_trajectory_status_[id] = ArmTrajectoryControllerNode::QUEUED;
   joint_trajectory_time_[id] = 0.0;
 }
 
@@ -520,7 +594,7 @@
     {
       joint_trajectory_vector_.erase(joint_trajectory_vector_.begin()+i);
       joint_trajectory_id_.erase(joint_trajectory_id_.begin()+i);
-      joint_trajectory_status_[id] = 3;
+      joint_trajectory_status_[id] = ArmTrajectoryControllerNode::DELETED;
       break;
     }
   }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9318
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9318&view=rev
Author:   sachinchitta
Date:     2009-01-13 03:42:05 +0000 (Tue, 13 Jan 2009)

Log Message:
-----------
grasp point node services a desired grasp point and generates joint angles for the point and another approach point at a standoff distance

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/grasp_point_node.h	2009-01-13 03:42:05 UTC (rev 9318)
@@ -0,0 +1,106 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <libKinematics/kinematics.h>
+#include <libKinematics/pr2_ik.h>
+
+#include <tf/tf.h>
+#include <tf/transform_listener.h>
+#include <LinearMath/btTransform.h>
+
+#include <pr2_msgs/GraspPoint.h>
+#include <pr2_mechanism_controllers/GraspPointSrv.h>
+
+#include <ros/node.h>
+
+#include <urdf/URDF.h>
+
+
+namespace grasp_point_node 
+{
+
+  #define NUM_JOINTS 7
+
+  class GraspPointNode : public ros::node
+  {
+    /**
+     * @brief Generate a arm trajectory to get to a grasp point
+     * access
+     */
+    public:  
+
+    GraspPointNode(std::string node_name);
+
+    ~GraspPointNode();
+  
+    void init();
+
+    private:
+
+    double increment_;
+
+    tf::TransformListener tf_; /**< Used to do transforms */
+
+    kinematics::arm7DOF *arm_kinematics_;
+
+    double grasp_standoff_distance_;
+
+    double init_solution_theta3_;
+
+    std::string robot_description_model_;
+
+    std::string service_prefix_;
+
+    std::string arm_name_;
+
+    std::string root_link_name_;
+
+    robot_desc::URDF urdf_model_;
+
+    robot_desc::URDF::Link* findNextLinkInGroup(robot_desc::URDF::Link *link_current, robot_desc::URDF::Group* group);
+
+    int initializeKinematicModel();
+
+    bool computeIKSolution(const tf::Pose &pose, std::vector<double> &soln);
+
+    tf::Transform calculateIntermediatePoint(tf::Transform grasp_point);  //Assumption is that the grasp point is located along the X axis of the grasp point transform
+
+    bool processGraspPointService(pr2_mechanism_controllers::GraspPointSrv::request &req, pr2_mechanism_controllers::GraspPointSrv::response &resp);
+
+    bool chooseSoln(const std::vector<std::vector<double> > &ik_solns, std::vector<double> &solution);
+
+  };
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2009-01-13 03:38:54 UTC (rev 9317)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2009-01-13 03:42:05 UTC (rev 9318)
@@ -22,6 +22,7 @@
   <depend package="angles" />
   <depend package="control_toolbox" />
   <depend package="filters" />
+  <depend package="libKinematics" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/grasp_point_node.cpp	2009-01-13 03:42:05 UTC (rev 9318)
@@ -0,0 +1,315 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <pr2_mechanism_controllers/grasp_point_node.h>
+
+using namespace grasp_point_node;
+
+GraspPointNode::GraspPointNode(std::string node_name):ros::node(node_name),tf_(*this, true, 10000000000ULL)
+{
+  service_prefix_ = node_name;
+}
+
+GraspPointNode::~GraspPointNode()
+{
+  this->unadvertise_service(service_prefix_ + "/SetGraspPoint");
+}
+
+void GraspPointNode::init()
+{
+  this->param<std::string>(service_prefix_ + "/robot_description",robot_description_model_,"robotdesc/pr2");
+  this->param<double>(service_prefix_ + "/grasp_standoff_distance",grasp_standoff_distance_,0.05);
+  this->param<std::string>(service_prefix_ + "/arm_name",arm_name_,"right_arm");
+  this->param<double>(service_prefix_ + "/increment",increment_,0.01);
+  initializeKinematicModel();
+
+  this->advertise_service(service_prefix_ + "/SetGraspPoint", &GraspPointNode::processGraspPointService, this);
+}
+
+robot_desc::URDF::Link* GraspPointNode::findNextLinkInGroup(robot_desc::URDF::Link *link_current, robot_desc::URDF::Group* group)
+{
+  std::vector<robot_desc::URDF::Link*>::iterator link_iter;
+
+#ifdef DEBUG
+  cout << "Current link:: " << link_current->name << endl; 
+#endif
+  for(link_iter = link_current->children.begin(); link_iter != link_current->children.end(); link_iter++)
+  {
+#ifdef DEBUG
+    cout << (*link_iter)->name;
+#endif
+    if((*link_iter)->insideGroup(group))
+      return *link_iter;
+  }
+  return NULL;
+}
+
+
+int GraspPointNode::initializeKinematicModel()
+{
+  std::vector<NEWMAT::Matrix> axis;
+  std::vector<NEWMAT::Matrix> anchor;
+  std::vector<std::string> joint_type;
+  std::vector<double> min_joint_limits;
+  std::vector<double> max_joint_limits;
+  std::vector<double> angle_multipliers;
+
+  NEWMAT::Matrix aj(3,1);
+  NEWMAT::Matrix an(3,1);
+
+  std::string xml_content;
+  std::vector<robot_desc::URDF::Group*> groups;
+
+
+  (ros::g_node)->get_param(robot_description_model_,xml_content);
+
+  // wait for robotdesc/pr2 on param server
+  while(!urdf_model_.loadString(xml_content.c_str()))
+  {
+    ROS_INFO("WARNING: grasp point node is waiting for robotdesc/pr2 in param server.  run roslaunch send.xml or similar.");
+    (ros::g_node)->get_param("robotdesc/pr2",xml_content);
+    usleep(100000);
+  }
+
+  urdf_model_.getGroups(groups);
+
+  int group_index = -1;
+
+  for(int i=0; i < (int) groups.size(); i++)
+  {
+    if(groups[i]->name == arm_name_)
+    {
+      group_index = i;
+      break;
+    }
+  }
+
+  if(!group_index) 
+    return -1;
+
+  if((int) groups[group_index]->linkRoots.size() != 1)
+  {
+    fprintf(stderr,"robot_kinematics.cpp::Too many roots in serial chain!\n");
+    return -1;
+  }
+
+  robot_desc::URDF::Link *link_current = groups[group_index]->linkRoots[0];
+  root_link_name_ = link_current->name;
+  joint_type.resize(NUM_JOINTS);
+
+  min_joint_limits.resize(NUM_JOINTS);
+  max_joint_limits.resize(NUM_JOINTS);
+  angle_multipliers.resize(NUM_JOINTS);
+  for(int i=0; i<NUM_JOINTS; i++)
+  {
+    aj << fabs(link_current->joint->axis[0]) << fabs(link_current->joint->axis[1]) << fabs(link_current->joint->axis[2]);
+    axis.push_back(aj);
+    if(i > 0)
+    {
+      an(1,1) = an(1,1) + link_current->xyz[0];
+      an(2,1) = an(2,1) + link_current->xyz[1];
+      an(3,1) = an(3,1) + link_current->xyz[2];
+    }
+    else
+    {
+      an(1,1) = 0.0;
+      an(2,1) = 0.0;
+      an(3,1) = 0.0;
+    }
+    anchor.push_back(an);   
+    min_joint_limits[i] = link_current->joint->limit[0];
+    max_joint_limits[i] = link_current->joint->limit[1];
+    angle_multipliers[i] = link_current->joint->axis[0]*fabs(link_current->joint->axis[0]) +  link_current->joint->axis[1]*fabs(link_current->joint->axis[1]) +  link_current->joint->axis[2]*fabs(link_current->joint->axis[2]);
+    ROS_INFO("Adding joint %s\naxis: %f %f %f\nanchor: %f %f %f",link_current->joint->name.c_str(),link_current->joint->axis[0],link_current->joint->axis[1],link_current->joint->axis[2],an(1,1),an(2,1),an(3,1));
+    if(min_joint_limits[i] == 0.0 && max_joint_limits[i] == 0.0)
+    {
+      ROS_INFO("Continuous joint");
+      min_joint_limits[i] = -M_PI;
+      max_joint_limits[i] = M_PI;
+    }
+    if(i==2)
+    {
+      init_solution_theta3_ = (min_joint_limits[i]+max_joint_limits[i])/2.0;
+      ROS_INFO("Initial guess for inverse kinematics: %f",init_solution_theta3_);
+    }
+    ROS_INFO("Joint limits %f, %f\n",min_joint_limits[i],max_joint_limits[i]);
+
+    link_current = findNextLinkInGroup(link_current, groups[group_index]);
+  }
+
+  for(int i=0; i < 7; i++)
+    joint_type[i] = std::string("ROTARY");
+
+  arm_kinematics_ = new kinematics::arm7DOF(anchor,axis,joint_type);
+  arm_kinematics_->SetJointLimits(min_joint_limits,max_joint_limits);
+  arm_kinematics_->increment_ = increment_;
+  arm_kinematics_->setAngleMultipliers(angle_multipliers);
+  return 1;
+}
+
+bool GraspPointNode::computeIKSolution(const tf::Pose &pose,std::vector<double> &soln)
+{
+  NEWMAT::Matrix g0(4,4);
+  btScalar m[16];
+  pose.getOpenGLMatrix(m);
+
+  ROS_INFO("computeIKSolution: Input transform");
+  for(int i=0; i< 4; i++)
+  {
+    for(int j=0; j<4; j++)
+    {
+      g0(j+1,i+1) = m[i*4+j];
+    }
+  }
+  std::cout << g0 << endl;
+
+  if(arm_kinematics_->computeIKFast(g0,2,init_solution_theta3_))
+  {
+    chooseSoln(arm_kinematics_->solution_ik_,soln);
+    return true;
+  }
+
+  return false;
+}
+
+
+bool GraspPointNode::chooseSoln(const std::vector<std::vector<double> > &ik_solns, std::vector<double> &solution)
+{
+
+  for(int i=0; i< (int) ik_solns.size(); i++)
+  {
+    for(int j=0; j<NUM_JOINTS; j++)
+    {
+      std::cout << ik_solns[i][j] << " ";
+    }
+    std::cout << std::endl;
+  }
+
+  for(int i=0; i< (int) ik_solns.size(); i++)
+  {
+    if(ik_solns[i][1] < 0)
+    {
+      for(int j=0; j<NUM_JOINTS; j++)
+      {
+        solution[j] = ik_solns[i][j];
+      }
+      init_solution_theta3_ = solution[2];
+      return true;
+    }
+  }
+  return false;
+}
+
+tf::Transform GraspPointNode::calculateIntermediatePoint(tf::Transform grasp_point)  //Assumption is that the grasp point is located along the X axis of the grasp point transform
+{
+  btMatrix3x3 rot;
+  rot.setIdentity();
+  tf::Transform x(rot,btVector3(-grasp_standoff_distance_,0.0,0.0));
+  tf::Transform result = grasp_point*x;
+  return result;
+}
+
+bool GraspPointNode::processGraspPointService(pr2_mechanism_controllers::GraspPointSrv::request &req, pr2_mechanism_controllers::GraspPointSrv::response &resp)
+{
+  std_msgs::PoseStamped grasp_point_transformed;
+  std_msgs::PoseStamped grasp_point_requested = req.transform;
+  tf::Pose grasp_point;
+
+  grasp_point_requested.header.stamp = ros::Time::now();
+
+  ROS_INFO("Joint::%s %s", req.transform.header.frame_id.c_str(),root_link_name_.c_str());
+  if(std::string(req.transform.header.frame_id) == root_link_name_)
+  {
+    grasp_point_transformed = req.transform;
+  }
+  else
+  {
+    try{
+      tf_.transformPose(root_link_name_,grasp_point_requested,grasp_point_transformed);
+    }
+    catch(tf::LookupException& ex) {
+      ROS_INFO("No Transform available Error\n");
+      return false;
+    }
+    catch(tf::ConnectivityException& ex) {
+      ROS_INFO("TF:: Connectivity Error\n");
+      return false;
+    }
+    catch(tf::ExtrapolationException& ex) {
+      ROS_INFO("Extrapolation Error\n");
+      return false;
+    }
+    catch(tf::TransformException e) {
+      return false;
+    }
+  }
+  tf::PoseMsgToTF(grasp_point_transformed.pose,grasp_point);
+  tf::Transform intermediate_point = calculateIntermediatePoint(grasp_point);
+
+  std::vector<double> soln[2];
+
+  soln[0].resize(NUM_JOINTS);
+  soln[1].resize(NUM_JOINTS);
+
+  if(!computeIKSolution(intermediate_point,soln[0]))
+    return false;
+  if(!computeIKSolution(grasp_point,soln[1]))
+    return false;
+
+  resp.traj.set_points_size(2);
+
+  for(int i=0; i<2;i++)
+  {
+    resp.traj.points[i].set_positions_size(NUM_JOINTS);
+    for(int j=0; j<NUM_JOINTS; j++)
+    {
+      resp.traj.points[i].positions[j] = soln[i][j];
+    }
+  }
+
+  return true;
+}
+
+using namespace grasp_point_node;
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv);
+  GraspPointNode graspnode("grasp_point_node");
+  graspnode.init();
+  graspnode.spin();
+  ros::fini();
+  return 0;
+}
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_grasp_point_node.cpp	2009-01-13 03:42:05 UTC (rev 9318)
@@ -0,0 +1,122 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <ros/node.h>
+#include <pr2_mechanism_controllers/GraspPointSrv.h>
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+
+static int done = 0;
+
+void finalize(int donecare)
+{
+  done = 1;
+}
+
+int main( int argc, char** argv )
+{
+
+  /*********** Initialize ROS  ****************/
+  ros::init(argc,argv);
+  ros::node *node = new ros::node("grasp_point_client"); 
+
+  signal(SIGINT,  finalize);
+  signal(SIGQUIT, finalize);
+  signal(SIGTERM, finalize);
+
+  pr2_mechanism_controllers::GraspPointSrv::request  req;
+  pr2_mechanism_controllers::GraspPointSrv::response res;
+
+  req.transform.header.stamp = ros::Time::now();
+  req.transform.header.frame_id = "r_shoulder_pan_link";
+  req.transform.pose.position.x = 0.5;
+  req.transform.pose.position.y = 0.0;
+  req.transform.pose.position.z = +0.2;
+
+  req.transform.pose.orientation.x = 0.0;
+  req.transform.pose.orientation.y = 0.0;
+  req.transform.pose.orientation.z = 0.0;
+  req.transform.pose.orientation.w = 1.0;
+
+  if (ros::service::call("/grasp_point_node/SetGraspPoint", req, res))
+  {
+    ROS_INFO("Done");
+
+    for(int i=0; i<2;i++)
+    {
+      for(int j=0; j< (int) res.traj.points[0].get_positions_size(); j++)
+      {
+        std::cout << res.traj.points[i].positions[j] << " ";
+      }
+      std::cout << std::endl;
+    }
+  }
+  else
+  {
+    ROS_INFO("IK failed");
+    return 0;
+  }
+
+  sleep(1);
+
+  pr2_mechanism_controllers::TrajectoryStart::request  reqt;
+  pr2_mechanism_controllers::TrajectoryStart::response rest;
+
+  int num_points = (int) res.traj.get_points_size();
+  int num_joints = (int) res.traj.points[0].get_positions_size();
+
+  reqt.traj.set_points_size(num_points);
+  reqt.requesttiming = 1;
+
+  for(int i=0; i<num_points; i++)
+  {
+    reqt.traj.points[i].set_positions_size(num_joints);
+    for(int j=0; j<num_joints; j++)
+    {
+      reqt.traj.points[i].positions[j] = res.traj.points[i].positions[j];
+    }
+  }
+  reqt.traj.points[1].time = 4.0;
+
+  if (ros::service::call("/right_arm_trajectory_controller/TrajectoryStart", reqt, rest))
+  {
+    ROS_INFO("Done");
+  }
+  else
+  {
+    ROS_INFO("Trajectory failed");
+  }
+//  sleep(20);
+  ros::fini();
+  return 0;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9494
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9494&view=rev
Author:   sachinchitta
Date:     2009-01-15 10:57:09 +0000 (Thu, 15 Jan 2009)

Log Message:
-----------
Added joint position field to trajectory query.

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-15 10:55:20 UTC (rev 9493)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-15 10:57:09 UTC (rev 9494)
@@ -464,10 +464,11 @@
                                                     pr2_mechanism_controllers::TrajectoryQuery::response &resp)
 {
   resp.set_jointnames_size(c_->dimension_);
+  resp.set_jointpositions_size(c_->dimension_);
   for(int i=0; i < c_->dimension_; i++)
   {
     resp.jointnames[i] = c_->joint_pd_controllers_[i]->getJointName();
-    
+    resp.jointpositions[i] = c_->joint_pd_controllers_[i]->joint_state_->position_;
   }
 
   if(req.trajectoryid == 0)

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv	2009-01-15 10:55:20 UTC (rev 9493)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryQuery.srv	2009-01-15 10:57:09 UTC (rev 9494)
@@ -16,3 +16,4 @@
 float32 trajectorytime # the current timestamp the trajectory is at. If the trajectory is finished, specifies the total time of the trajectory
 
 string[] jointnames # names of the joints that the controller expects
+float64[] jointpositions # joint values
\ No newline at end of file


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9812
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9812&view=rev
Author:   sachinchitta
Date:     2009-01-20 22:32:56 +0000 (Tue, 20 Jan 2009)

Log Message:
-----------
added controller state message to controller

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-20 22:30:43 UTC (rev 9811)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-20 22:32:56 UTC (rev 9812)
@@ -66,6 +66,10 @@
 #include <robot_kinematics/robot_kinematics.h>
 #include <trajectory/trajectory.h>
 
+#include <robot_msgs/ControllerState.h>
+#include "realtime_tools/realtime_publisher.h"
+#include "std_msgs/String.h"
+
 // #include <libTF/Pose3D.h>
 // #include <urdf/URDF.h>
 
@@ -157,6 +161,10 @@
     double trajectory_wait_time_;
 
     std::vector<double> goal_reached_threshold_;
+
+    realtime_tools::RealtimePublisher <robot_msgs::ControllerState>* controller_state_publisher_ ;  //!< Publishes controller information
+
+    double max_update_time_;
   };
 
 /** @class ArmTrajectoryControllerNode

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-20 22:30:43 UTC (rev 9811)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-20 22:32:56 UTC (rev 9812)
@@ -35,8 +35,6 @@
 // Original version: Sachin Chitta <sachinc@...>
 
 #include "pr2_mechanism_controllers/arm_trajectory_controller.h"
-#include "realtime_tools/realtime_publisher.h"
-#include "std_msgs/String.h"
 
 using namespace controller;
 using namespace std;
@@ -51,8 +49,9 @@
 }
 
 ArmTrajectoryController::ArmTrajectoryController() :
-  refresh_rt_vals_(false),trajectory_type_("linear"),trajectory_wait_time_(0.0)
+  refresh_rt_vals_(false),trajectory_type_("linear"),trajectory_wait_time_(0.0), max_update_time_(0.0)
 {
+  controller_state_publisher_ = NULL;
 }
 
 ArmTrajectoryController::~ArmTrajectoryController()
@@ -145,7 +144,7 @@
   }
 
   joint_trajectory_->setTrajectory(joint_trajectory);
-  joint_trajectory_->write("foo.txt",0.01);
+//  joint_trajectory_->write("foo.txt",0.01);
   arm_controller_lock_.lock();
   refresh_rt_vals_ = true;
   arm_controller_lock_.unlock();
@@ -227,7 +226,16 @@
   updateJointControllers();
 
   double end_time = now();
+  max_update_time_ = std::max(max_update_time_,end_time-start_time);
 
+  if (controller_state_publisher_->trylock())
+  {
+    controller_state_publisher_->msg_.update_time = end_time - start_time; 
+    controller_state_publisher_->msg_.max_update_time = max_update_time_; 
+    controller_state_publisher_->unlockAndPublish();      
+  }
+
+/*
   static realtime_tools::RealtimePublisher<std_msgs::String> p("/s", 1);
   if (p.trylock()) {
     char buf[1000];
@@ -235,6 +243,7 @@
     p.msg_.data = std::string(buf);
     p.unlockAndPublish();
   }
+*/
 }
 
 bool ArmTrajectoryController::reachedGoalPosition(std::vector<double> joint_cmd)
@@ -275,6 +284,9 @@
   node_->unadvertiseService(service_prefix_ + "/TrajectoryStart");
   node_->unadvertiseService(service_prefix_ + "/TrajectoryQuery");
 
+  c_->controller_state_publisher_->stop();
+  delete c_->controller_state_publisher_;
+
    if(topic_name_ptr_ && topic_name_.c_str())
   {
     std::cout << "unsub arm controller" << topic_name_ << std::endl;
@@ -293,6 +305,8 @@
   {
     updateTrajectoryQueue( ArmTrajectoryControllerNode::FAILED);
   }
+
+
   c_->update();
 }
 
@@ -359,6 +373,15 @@
 
     getJointTrajectoryThresholds();
 
+
+  if (c_->controller_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete c_->controller_state_publisher_ ;
+  c_->controller_state_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::ControllerState> (service_prefix_+"/controller_state", 1) ;
+
+  ROS_INFO("Initialize publisher");
+
+  c_->controller_state_publisher_->msg_.name = std::string(service_prefix_); 
+
     ROS_INFO("Initialized controller");
     return true;
   }
@@ -604,4 +627,3 @@
     }
   }
 }
-


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 9969
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9969&view=rev
Author:   isucan
Date:     2009-01-22 19:04:08 +0000 (Thu, 22 Jan 2009)

Log Message:
-----------
using realtime_gettime() instead of clock_gettime() and made time publishing optional (a #define)

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-22 18:37:07 UTC (rev 9968)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-22 19:04:08 UTC (rev 9969)
@@ -67,17 +67,20 @@
 #include <trajectory/trajectory.h>
 
 #include <robot_msgs/ControllerState.h>
-#include "realtime_tools/realtime_publisher.h"
-#include "std_msgs/String.h"
+#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_tools.h>
+#include <std_msgs/String.h>
 
-// #include <libTF/Pose3D.h>
-// #include <urdf/URDF.h>
-
 namespace controller
 {
 
   #define GOAL_REACHED_THRESHOLD 0.01
 
+
+    // comment this out if the controller is not supposed to publish its own max execution time
+  #define PUBLISH_MAX_TIME
+
+ 
 // The maximum number of joints expected in an arm.
   static const int MAX_ARM_JOINTS = 7;
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-22 18:37:07 UTC (rev 9968)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-22 19:04:08 UTC (rev 9969)
@@ -41,12 +41,6 @@
 
 ROS_REGISTER_CONTROLLER(ArmTrajectoryController);
 
-static inline double now()
-{
-  struct timespec n;
-  clock_gettime(CLOCK_MONOTONIC, &n);
-  return double(n.tv_nsec) / 1.0e9 + n.tv_sec;
-}
 
 ArmTrajectoryController::ArmTrajectoryController() :
   refresh_rt_vals_(false),trajectory_type_("linear"),trajectory_wait_time_(0.0), max_update_time_(0.0)
@@ -180,8 +174,11 @@
 
 void ArmTrajectoryController::update(void)
 {
-  double start_time = now();
 
+#ifdef PUBLISH_MAX_TIME
+  double start_time = realtime_gettime();
+#endif
+
   double sample_time(0.0);
 
   current_time_ = robot_->hw_->current_time_;
@@ -224,8 +221,9 @@
     joint_pd_controllers_[i]->setCommand(joint_cmd_rt_[i],joint_cmd_dot_rt_[i]);
 
   updateJointControllers();
-
-  double end_time = now();
+  
+#ifdef PUBLISH_MAX_TIME
+  double end_time = realtime_gettime();
   max_update_time_ = std::max(max_update_time_,end_time-start_time);
 
   if (controller_state_publisher_->trylock())
@@ -234,16 +232,8 @@
     controller_state_publisher_->msg_.max_update_time = max_update_time_; 
     controller_state_publisher_->unlockAndPublish();      
   }
+#endif
 
-/*
-  static realtime_tools::RealtimePublisher<std_msgs::String> p("/s", 1);
-  if (p.trylock()) {
-    char buf[1000];
-    sprintf(buf, "Time = %15.6lf\n", end_time - start_time);
-    p.msg_.data = std::string(buf);
-    p.unlockAndPublish();
-  }
-*/
 }
 
 bool ArmTrajectoryController::reachedGoalPosition(std::vector<double> joint_cmd)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 10032
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10032&view=rev
Author:   sachinchitta
Date:     2009-01-23 01:57:10 +0000 (Fri, 23 Jan 2009)

Log Message:
-----------
Base controller now transmits a STUCK message for each joint, parameters for which can be adjusted through roslaunch

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-01-23 01:56:57 UTC (rev 10031)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-01-23 01:57:10 UTC (rev 10032)
@@ -57,6 +57,7 @@
 #include <std_msgs/BaseVel.h>
 #include <pr2_msgs/Odometer.h>
 #include <pr2_msgs/Covariance2D.h>
+#include <pr2_msgs/BaseControllerState.h>
 
 #include <realtime_tools/realtime_publisher.h>
 
@@ -266,6 +267,36 @@
 
     private:
 
+    void computeStall();
+
+    std::vector<double> caster_speed_;
+
+    std::vector<double> caster_position_error_;
+
+    std::vector<double> caster_speed_filtered_;
+
+    std::vector<double> caster_speed_error_;
+
+    std::vector<int> caster_stuck_;
+
+    double caster_speed_threshold_;
+
+    double caster_effort_threshold_;
+
+    std::vector<double> wheel_speed_error_;
+
+    std::vector<double> wheel_speed_filtered_;
+
+    std::vector<int> wheel_stuck_;
+
+    double wheel_speed_threshold_;
+
+    double wheel_effort_threshold_;
+
+    double caster_position_error_threshold_;
+
+    double alpha_stall_;
+
     bool new_cmd_available_; /** true when new command received by node */
 
     /*!
@@ -489,6 +520,8 @@
 
     realtime_tools::RealtimePublisher <pr2_msgs::Covariance2D>* covariance_publisher_ ;  //!< Publishes the odom to base transform msg from the update() realtime loop
 
+    realtime_tools::RealtimePublisher <pr2_msgs::BaseControllerState>* state_publisher_ ;  
+
     /*
      * \brief pointer to ros node
      */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-01-23 01:56:57 UTC (rev 10031)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-01-23 01:57:10 UTC (rev 10032)
@@ -247,6 +247,13 @@
       steer_angle_stored_.push_back(0);
       steer_velocity_desired_.push_back(0);
       num_casters_++;
+
+      caster_speed_.push_back(0.0);
+      caster_speed_filtered_.push_back(0.0);
+      caster_speed_error_.push_back(0.0);
+      caster_position_error_.push_back(0.0);
+      caster_stuck_.push_back(0);
+
 //      cout << "base_casters" << "::  " << base_object;
     }
 
@@ -262,6 +269,11 @@
       libTF::Vector *v=new libTF::Vector();
       base_wheels_position_.push_back(*v);
       wheel_speed_cmd_.push_back(0);
+
+      wheel_speed_filtered_.push_back(0.0);
+      wheel_speed_error_.push_back(0.0);
+      wheel_stuck_.push_back(0);
+
       num_wheels_++;
     }
   }
@@ -455,6 +467,8 @@
 
   updateJointControllers();
 
+  computeStall();
+
   last_time_ = current_time;
 }
 
@@ -506,7 +520,7 @@
       steer_angle_desired = steer_angle_desired_m_pi;
     }
     steer_velocity_desired_[i] =  -kp_speed_*error_steer;
-
+    caster_position_error_[i] = error_steer;
   }
 }
 
@@ -612,7 +626,6 @@
 }
 
 
-
 void BaseController::computeBaseVelocity()
 {
   Matrix A(2*num_wheels_,1);
@@ -679,12 +692,14 @@
   return result;
 }
 
+
 std::ostream & controller::operator<<(std::ostream& mystream, const controller::BaseParam &bp)
 {
   mystream << "BaseParam" << bp.name_ << endl << "position " << bp.pos_ << "id " << bp.local_id_ << endl << "joint " << bp.joint_state_->joint_->name_ << endl << endl;
   return mystream;
 };
 
+
 NEWMAT::Matrix BaseController::iterativeLeastSquares(NEWMAT::Matrix A, NEWMAT::Matrix b, std::string weight_type, int max_iter)
 {
   NEWMAT::IdentityMatrix ident_matrix(2*num_wheels_);
@@ -723,6 +738,7 @@
   return x_fit;
 };
 
+
 NEWMAT::Matrix BaseController::findWeightMatrix(NEWMAT::Matrix residual, std::string weight_type)
 {
   NEWMAT::Matrix w_fit(2*num_wheels_,2*num_wheels_);
@@ -772,9 +788,40 @@
 };
 
 
+void BaseController::computeStall()
+{
 
+  for(int i=0; i < num_casters_; i++)
+  {     
+    caster_speed_error_[i] = fabs(base_casters_[i].joint_state_->velocity_ - steer_velocity_desired_[i]);
+    caster_speed_filtered_[i] = alpha_stall_*caster_speed_filtered_[i] + (1-alpha_stall_)*base_casters_[i].joint_state_->velocity_;
 
+    if(fabs(caster_speed_[i]) < caster_speed_threshold_ &&  fabs(caster_position_error_[i]) > caster_position_error_threshold_ && fabs(base_casters_[i].joint_state_->applied_effort_) > caster_effort_threshold_)
+    {
+        caster_stuck_[i] = 1;
+    }
+    else
+    {
+      caster_stuck_[i] = 0;
+    }
+  }
+  for(int j =0; j < num_wheels_; j++)
+  {
+    wheel_speed_error_[j] = fabs(base_wheels_[j].joint_state_->velocity_ - wheel_speed_cmd_[j]);
+    wheel_speed_filtered_[j] = alpha_stall_*wheel_speed_filtered_[j] + (1-alpha_stall_)*wheel_speed_actual_[j];
+    if(fabs(wheel_speed_filtered_[j]) < wheel_speed_threshold_ && fabs(base_wheels_[j].joint_state_->applied_effort_) > wheel_effort_threshold_)
+    {
+      wheel_stuck_[j] = 1;
+    }
+    else
+    {
+      wheel_stuck_[j] = 0;
+    }
+  }
+}
 
+
+
 ROS_REGISTER_CONTROLLER(BaseControllerNode)
 
   BaseControllerNode::BaseControllerNode()
@@ -788,6 +835,7 @@
   transform_publisher_ = NULL;
   odometer_publisher_ = NULL;
   covariance_publisher_ = NULL;
+  state_publisher_ = NULL;
 }
 
 BaseControllerNode::~BaseControllerNode()
@@ -803,11 +851,15 @@
   transform_publisher_->stop();
   odometer_publisher_->stop();
   covariance_publisher_->stop();
+  state_publisher_->stop();
 
   delete publisher_;
   delete transform_publisher_;
   delete odometer_publisher_;
   delete covariance_publisher_;
+
+  delete state_publisher_;
+
   delete c_;
 }
 
@@ -835,6 +887,33 @@
       last_time_message_sent_ = time;
     }
 
+    if(state_publisher_->trylock())
+    {
+      state_publisher_->msg_.command_vx = c_->cmd_vel_.x;
+      state_publisher_->msg_.command_vy = c_->cmd_vel_.y;
+      state_publisher_->msg_.command_vw = c_->cmd_vel_.z;
+      for(int i=0; i<c_->num_casters_; i++)
+      {
+        state_publisher_->msg_.joint_speed[i] = c_->caster_speed_[i];
+        state_publisher_->msg_.joint_speed_filtered[i] = c_->caster_speed_filtered_[i];
+        state_publisher_->msg_.joint_speed_error[i] = c_->caster_speed_error_[i];
+        state_publisher_->msg_.joint_stall[i] = c_->caster_stuck_[i];
+        state_publisher_->msg_.joint_commanded_effort[i] = c_->base_casters_[i].joint_state_->commanded_effort_;
+        state_publisher_->msg_.joint_applied_effort[i] = c_->base_casters_[i].joint_state_->applied_effort_;
+      }
+      for(int i=0; i<c_->num_wheels_; i++)
+      {
+        state_publisher_->msg_.joint_speed[i+c_->num_casters_] = c_->wheel_speed_actual_[i];
+        state_publisher_->msg_.joint_speed_filtered[i+c_->num_casters_] = c_->wheel_speed_filtered_[i];
+        state_publisher_->msg_.joint_speed_error[i+c_->num_casters_] = c_->wheel_speed_error_[i];
+        state_publisher_->msg_.joint_stall[i+c_->num_casters_] = c_->wheel_stuck_[i];
+        state_publisher_->msg_.joint_commanded_effort[i+c_->num_casters_] = c_->base_wheels_[i].joint_state_->commanded_effort_;
+        state_publisher_->msg_.joint_applied_effort[i+c_->num_casters_] = c_->base_wheels_[i].joint_state_->applied_effort_;
+      }
+
+      state_publisher_->unlockAndPublish() ;
+    }
+
     if (covariance_publisher_->trylock())
     {
       double base_odom_velocity_mag = sqrt(c_->base_odom_velocity_.x*c_->base_odom_velocity_.x+c_->base_odom_velocity_.y*c_->base_odom_velocity_.y+c_->base_odom_velocity_.z*c_->base_odom_velocity_.z);
@@ -1008,6 +1087,10 @@
     delete odometer_publisher_ ;
   odometer_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::Odometer> (service_prefix + "/odometer", 1) ;
 
+  if (state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete state_publisher_;
+  state_publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::BaseControllerState> (service_prefix + "/state", 1) ;
+
   if (transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete transform_publisher_ ;
   transform_publisher_ = new realtime_tools::RealtimePublisher <tf::tfMessage> ("tf_message", 5) ;
@@ -1019,12 +1102,40 @@
 
   node->param<double>("base_controller/odom_publish_rate",odom_publish_rate_,100);
 
+  node->param<double>("base_controller/caster_speed_threshold",c_->caster_speed_threshold_,0.2);
+  node->param<double>("base_controller/caster_position_error_threshold",c_->caster_position_error_threshold_,0.05);
+  node->param<double>("base_controller/wheel_speed_threshold",c_->wheel_speed_threshold_,0.2);
+
+  node->param<double>("base_controller/caster_effort_threshold",c_->caster_effort_threshold_,3.45);
+  node->param<double>("base_controller/wheel_effort_threshold",c_->wheel_effort_threshold_,3.45);
+
+  node->param<double>("base_controller/alpha_stall",c_->alpha_stall_,0.5);
+
   double multiplier;
   node->param<double>("base_controller/wheel_radius_multiplier",multiplier,1.0);
   c_->wheel_radius_ = c_->wheel_radius_*multiplier;
 
   transform_publisher_->msg_.set_transforms_size(NUM_TRANSFORMS);
 
+  state_publisher_->msg_.set_joint_name_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_stall_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_speed_error_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_speed_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_speed_filtered_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_commanded_effort_size(c_->num_wheels_+c_->num_casters_);
+  state_publisher_->msg_.set_joint_applied_effort_size(c_->num_wheels_+c_->num_casters_);
+
+  for(int i=0; i < c_->num_casters_; i++)
+  {
+    state_publisher_->msg_.joint_name[i] = c_->base_casters_[i].name_;
+  }
+
+  for(int j=c_->num_casters_; j < c_->num_casters_+c_->num_wheels_; j++)
+  {
+    state_publisher_->msg_.joint_name[j] = c_->base_wheels_[j-c_->num_casters_].name_;
+  }
+
+
   if(odom_publish_rate_ > 1e-5)
     odom_publish_delta_t_ = 1.0/odom_publish_rate_;
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 10320
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10320&view=rev
Author:   sachinchitta
Date:     2009-01-28 01:59:53 +0000 (Wed, 28 Jan 2009)

Log Message:
-----------
switching from trajectory messages in controllers to robot_msgs

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-28 01:59:07 UTC (rev 10319)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-01-28 01:59:53 UTC (rev 10320)
@@ -56,8 +56,8 @@
 #include <pr2_mechanism_controllers/SetJointTarget.h>
 #include <pr2_mechanism_controllers/JointPosCmd.h>
 
-#include <pr2_mechanism_controllers/JointTraj.h>
-#include <pr2_mechanism_controllers/JointTrajPoint.h>
+#include <robot_msgs/JointTraj.h>
+#include <robot_msgs/JointTrajPoint.h>
 
 #include <pr2_mechanism_controllers/TrajectoryStart.h>
 #include <pr2_mechanism_controllers/TrajectoryQuery.h>
@@ -220,9 +220,9 @@
 
     void deleteTrajectoryFromQueue(int id);
 
-    void addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id);
+    void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
 
-    int createTrajectory(const pr2_mechanism_controllers::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+    int createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
 
     void updateTrajectoryQueue(int last_trajectory_finish_status);
 
@@ -235,7 +235,7 @@
      */
     boost::mutex ros_lock_;
 
-    pr2_mechanism_controllers::JointTraj traj_msg_;
+    robot_msgs::JointTraj traj_msg_;
 
     pr2_mechanism_controllers::JointPosCmd msg_;   //The message used by the ROS callback
     ArmTrajectoryController *c_;
@@ -268,11 +268,11 @@
 
     int trajectory_id_;
 
-    std::vector<pr2_mechanism_controllers::JointTraj> joint_trajectory_vector_;
+    std::vector<robot_msgs::JointTraj> joint_trajectory_vector_;
 
     std::vector<int> joint_trajectory_id_;
 
-    void setTrajectoryCmdFromMsg(pr2_mechanism_controllers::JointTraj traj_msg);
+    void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg);
 
     int request_trajectory_id_;
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2009-01-28 01:59:07 UTC (rev 10319)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml	2009-01-28 01:59:53 UTC (rev 10320)
@@ -15,6 +15,7 @@
   <depend package="libTF" />
   <depend package="std_msgs" />
   <depend package="pr2_msgs" />
+  <depend package="robot_msgs" />
   <depend package="robot_kinematics" />
   <depend package="rosconsole" />
   <depend package="trajectory" />

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-28 01:59:07 UTC (rev 10319)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-01-28 01:59:53 UTC (rev 10320)
@@ -388,7 +388,7 @@
   }
 }
 
-void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(pr2_mechanism_controllers::JointTraj traj_msg)
+void ArmTrajectoryControllerNode::setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg)
 {
   std::vector<trajectory::Trajectory::TPoint> tp;
   tp.resize((int)traj_msg.get_points_size()+1);
@@ -555,7 +555,7 @@
   return true;
 }
 
-int ArmTrajectoryControllerNode::createTrajectory(const pr2_mechanism_controllers::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
+int ArmTrajectoryControllerNode::createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
 {
   std::vector<trajectory::Trajectory::TPoint> tp;
 
@@ -595,7 +595,7 @@
   return 1;
 }
 
-void ArmTrajectoryControllerNode::addTrajectoryToQueue(pr2_mechanism_controllers::JointTraj new_traj, int id)
+void ArmTrajectoryControllerNode::addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id)
 {
   joint_trajectory_vector_.push_back(new_traj);
   joint_trajectory_id_.push_back(id);

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv	2009-01-28 01:59:07 UTC (rev 10319)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/TrajectoryStart.srv	2009-01-28 01:59:53 UTC (rev 10320)
@@ -1,5 +1,5 @@
 ## Starts a trajectory
-JointTraj traj
+robot_msgs/JointTraj traj
 uint8 hastiming # if 1, use timestamps of trajectory points, otherwise do internal retiming
 uint8 requesttiming # if 1, send back the timestamps for every trajectory point (first point starts at 0)
 ---


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 10354
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10354&view=rev
Author:   tfoote
Date:     2009-01-28 04:40:30 +0000 (Wed, 28 Jan 2009)

Log Message:
-----------
fixing the build

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv	2009-01-28 04:33:46 UTC (rev 10353)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/GraspPointSrv.srv	2009-01-28 04:40:30 UTC (rev 10354)
@@ -1,3 +1,3 @@
 std_msgs/PoseStamped transform
 ---
-pr2_mechanism_controllers/JointTraj traj
\ No newline at end of file
+robot_msgs/JointTraj traj
\ No newline at end of file

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-28 04:33:46 UTC (rev 10353)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-01-28 04:40:30 UTC (rev 10354)
@@ -33,7 +33,7 @@
  *********************************************************************/
 
 #include <ros/node.h>
-#include <pr2_mechanism_controllers/JointTraj.h>
+#include <robot_msgs/JointTraj.h>
 
 static int done = 0;
 
@@ -55,7 +55,7 @@
 
 
   /*********** Start moving the robot ************/
-  pr2_mechanism_controllers::JointTraj cmd;
+  robot_msgs::JointTraj cmd;
 
   int num_points = 3;
   int num_joints = 7;
@@ -103,7 +103,7 @@
   cmd.points[2].positions[6] = 0.0;
   cmd.points[2].time = 0.0;
 
-  node->advertise<pr2_mechanism_controllers::JointTraj>("arm_trajectory_command",1);
+  node->advertise<robot_msgs::JointTraj>("arm_trajectory_command",1);
   sleep(1);
   node->publish("arm_trajectory_command",cmd);
   sleep(4);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 11488
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11488&view=rev
Author:   rob_wheeler
Date:     2009-02-21 00:05:31 +0000 (Sat, 21 Feb 2009)

Log Message:
-----------
Cleanup compiler warnings

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-02-20 23:58:01 UTC (rev 11487)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-02-21 00:05:31 UTC (rev 11488)
@@ -82,7 +82,7 @@
 
     friend std::ostream & operator<<(std::ostream& mystream, const controller::BaseParam &bp);
 
-    BaseParam():direction_multiplier_(1),name_(" "),joint_state_(NULL),parent_(NULL){};
+    BaseParam():name_(" "),joint_state_(NULL),parent_(NULL),direction_multiplier_(1){};
 
     ~BaseParam(){}
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-02-20 23:58:01 UTC (rev 11487)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-02-21 00:05:31 UTC (rev 11488)
@@ -964,7 +964,7 @@
 
       //      out.transform.rotation.x = 0;
       //      out.transform.rotation.y = 0;
-      double angle = angles::normalize_angle(-yaw);
+      /*double angle =*/ angles::normalize_angle(-yaw);
       //      out.transform.rotation.z = sqrt(1/(1 + pow(angle, 2)));
       //      out.transform.rotation.w = sqrt(pow(angle, 2) / (1 + pow(angle, 2)));
       //      out.pitch = 0;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-02-20 23:58:01 UTC (rev 11487)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-02-21 00:05:31 UTC (rev 11488)
@@ -1020,7 +1020,7 @@
       out.transform.translation.z = 0;
       out.transform.rotation.x = 0;
       out.transform.rotation.y = 0;
-      double angle = angles::normalize_angle(-yaw);
+      /*double angle =*/ angles::normalize_angle(-yaw);
 
 
       tf::Quaternion quat_trans = tf::Quaternion(-yaw,0.0,0.0);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 11798
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11798&view=rev
Author:   gerkey
Date:     2009-02-26 03:07:41 +0000 (Thu, 26 Feb 2009)

Log Message:
-----------
Fixed CancelTrajectory service

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-02-26 02:56:12 UTC (rev 11797)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-02-26 03:07:41 UTC (rev 11798)
@@ -61,6 +61,7 @@
 
 #include <pr2_mechanism_controllers/TrajectoryStart.h>
 #include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
 
 //Kinematics
 #include <robot_kinematics/robot_kinematics.h>
@@ -217,8 +218,8 @@
     bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
                            pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
 
-    bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
-                            pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
+    bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
+                            pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
 
     void deleteTrajectoryFromQueue(int id);
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-02-26 02:56:12 UTC (rev 11797)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-02-26 03:07:41 UTC (rev 11798)
@@ -347,6 +347,7 @@
 
    node_->advertiseService(service_prefix_ + "/TrajectoryStart", &ArmTrajectoryControllerNode::setJointTrajSrv, this);
    node_->advertiseService(service_prefix_ + "/TrajectoryQuery", &ArmTrajectoryControllerNode::queryJointTrajSrv, this);
+   node_->advertiseService(service_prefix_ + "/TrajectoryCancel", &ArmTrajectoryControllerNode::cancelJointTrajSrv, this);
 
     topic_name_ptr_ = config->FirstChildElement("listen_topic");
     if(topic_name_ptr_)
@@ -531,8 +532,8 @@
   return true;
 }
 
-bool ArmTrajectoryControllerNode::cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
-                                                     pr2_mechanism_controllers::TrajectoryQuery::Response &resp)
+bool ArmTrajectoryControllerNode::cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
+                                                     pr2_mechanism_controllers::TrajectoryCancel::Response &resp)
 {
   int status = ArmTrajectoryControllerNode::NUM_STATUS;
 
@@ -548,7 +549,7 @@
   {
     deleteTrajectoryFromQueue(req.trajectoryid);
   }
-  else if(resp.done == ArmTrajectoryControllerNode::ACTIVE)
+  else if(status == ArmTrajectoryControllerNode::ACTIVE)
   {
     updateTrajectoryQueue(CANCELED);
     // Add two points since every good trajectory must have at least two points, otherwise its just a point :-)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12035
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12035&view=rev
Author:   sachinchitta
Date:     2009-03-03 04:52:40 +0000 (Tue, 03 Mar 2009)

Log Message:
-----------
first attempt at creating a whole body position trajectory controller

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-03 04:51:14 UTC (rev 12034)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-03 04:52:40 UTC (rev 12035)
@@ -24,6 +24,7 @@
   src/wrist_calibration_controller.cpp
   src/gripper_calibration_controller.cpp
   src/arm_trajectory_controller.cpp
+  src/whole_body_trajectory_controller.cpp
   )
 rospack_link_boost(pr2_mechanism_controllers thread)
 

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	2009-03-03 04:52:40 UTC (rev 12035)
@@ -0,0 +1,326 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <boost/thread/mutex.hpp>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_position_controller.h>
+#include <robot_mechanism_controllers/joint_velocity_controller.h>
+#include <robot_mechanism_controllers/joint_effort_controller.h>
+#include <robot_mechanism_controllers/joint_pd_controller.h>
+
+// Services
+#include <pr2_mechanism_controllers/SetJointPosCmd.h>
+#include <pr2_mechanism_controllers/GetJointPosCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointGains.h>
+#include <pr2_mechanism_controllers/GetJointGains.h>
+
+#include <pr2_mechanism_controllers/SetCartesianPosCmd.h>
+#include <pr2_mechanism_controllers/GetCartesianPosCmd.h>
+
+#include <pr2_mechanism_controllers/SetJointTarget.h>
+#include <pr2_mechanism_controllers/JointPosCmd.h>
+
+#include <robot_msgs/JointTraj.h>
+#include <robot_msgs/JointTrajPoint.h>
+
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+
+#include <pr2_mechanism_controllers/base_controller.h>
+
+//Kinematics
+#include <robot_kinematics/robot_kinematics.h>
+#include <trajectory/trajectory.h>
+
+#include <robot_msgs/ControllerState.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_tools.h>
+#include <std_msgs/String.h>
+
+#include <angles/angles.h>
+
+namespace controller
+{
+
+  #define GOAL_REACHED_THRESHOLD 0.01
+
+
+    // comment this out if the controller is not supposed to publish its own max execution time
+  #define PUBLISH_MAX_TIME
+
+ 
+// The maximum number of joints expected in an arm.
+  static const int MAX_ARM_JOINTS = 7;
+
+  class WholeBodyTrajectoryController : public Controller
+  {
+    public:
+
+    /*!
+     * \brief Default Constructor of the JointController class.
+     *
+     */
+    WholeBodyTrajectoryController();
+
+    /*!
+     * \brief Destructor of the JointController class.
+     */
+    virtual ~WholeBodyTrajectoryController();
+
+    /*!
+     * \brief Functional way to initialize limits and gains.
+     */
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * \brief set the joint trajectory for the arm
+     */
+    void setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
+
+    void getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const;
+
+    /*!
+     * \brief Issues commands to the joint. Should be called at regular intervals
+     */
+    virtual void update(void); // Real time safe.
+
+    boost::mutex arm_controller_lock_;
+
+    controller::JointPDController* getJointControllerByName(std::string name);
+
+    private:
+
+    double last_time_;
+
+    std::vector<control_toolbox::Pid> base_pid_controller_;      /**< Internal PID controller. */
+
+    std::vector<int> base_joint_index_;
+
+    std::vector<std::string> joint_name_;
+
+    std::vector<int> joint_type_;
+
+    std::vector<double> current_joint_position_;
+
+    std::vector<double> current_joint_velocity_;
+
+    controller::BaseControllerNode base_controller_node_;
+
+    std::vector<JointPDController *> joint_pv_controllers_;
+
+    trajectory::Trajectory *joint_trajectory_;
+
+    trajectory::Trajectory::TPoint trajectory_point_;
+
+    std::vector<double> joint_cmd_rt_;
+
+    std::vector<double> joint_cmd_dot_rt_;
+
+    mechanism::Robot* robot_;
+
+    void updateJointControllers(void);
+
+    void updateBaseController(double time);
+
+    void updateJointValues();
+
+    bool reachedGoalPosition(std::vector<double> joint_cmd);
+
+    int getJointControllerPosByName(std::string name);
+
+    // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
+    bool refresh_rt_vals_;
+
+    double trajectory_start_time_;
+
+    double trajectory_end_time_;
+
+    double current_time_;
+
+    bool trajectory_done_;
+
+    int dimension_;
+
+    std::vector<double> joint_velocity_limits_;
+
+    std::string trajectory_type_;
+
+    double velocity_scaling_factor_;
+
+    friend class WholeBodyTrajectoryControllerNode;
+
+    double trajectory_wait_time_;
+
+    std::vector<double> goal_reached_threshold_;
+
+    realtime_tools::RealtimePublisher <robot_msgs::ControllerState>* controller_state_publisher_ ;  //!< Publishes controller information
+
+    double max_update_time_;
+  };
+
+/** @class WholeBodyTrajectoryControllerNode
+ *  @brief ROS interface for the whole body trajectory controller.
+ *  @author Sachin Chitta <sachinc@...>
+ *
+ *  This class provides a ROS interface for controlling the arm by setting position configurations. If offers several ways to control the arms:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single configuration as a target (and maybe multiple configuration in the future)
+ *  - through a blocking service call: this service can receive a list of position commands that will be followed one after the other
+ * @note This controller makes the assumptiom that a point update is real-time safe.
+ * This is not the case for example for the LQR controller.
+ *
+ */
+  class WholeBodyTrajectoryControllerNode : public Controller
+  {
+    public:
+    /*!
+     * \brief Default Constructor
+     *
+     */
+    WholeBodyTrajectoryControllerNode();
+
+    /*!
+     * \brief Destructor
+     */
+    ~WholeBodyTrajectoryControllerNode();
+
+    void update();
+
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /** @brief service that returns the goal of the controller
+     * @note if you know the goal has been reached and you do not want to subscribe to the /mechanism_state topic, you can use it as a hack to get the position of the arm
+     * @param req
+     * @param resp the response, contains a JointPosCmd message with the goal of the controller
+     * @return
+     */
+    bool getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
+                        pr2_mechanism_controllers::GetJointPosCmd::Response &resp);
+
+    bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
+                         pr2_mechanism_controllers::TrajectoryStart::Response &resp);
+
+    bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
+                           pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
+
+    bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
+                            pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
+
+    void deleteTrajectoryFromQueue(int id);
+
+    void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
+
+    int createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+
+    void updateTrajectoryQueue(int last_trajectory_finish_status);
+
+    void getJointTrajectoryThresholds();
+
+    private:
+
+    /*!
+     * \brief mutex lock for setting and getting ros messages
+     */
+    boost::mutex ros_lock_;
+
+    robot_msgs::JointTraj traj_msg_;
+
+    pr2_mechanism_controllers::JointPosCmd msg_;   //The message used by the ROS callback
+    WholeBodyTrajectoryController *c_;
+
+    /*!
+     * \brief service prefix
+     */
+    std::string service_prefix_;
+
+    /*
+     * \brief save topic name for unsubscribe later
+     */
+    std::string topic_name_;
+
+    /*!
+     * \brief xml pointer to ros topic name
+     */
+    TiXmlElement * topic_name_ptr_;
+
+    /*
+     * \brief pointer to ros node
+     */
+    ros::Node * const node_;
+
+
+    /*
+     * \brief receive and set the trajectory in the controller
+     */
+    void CmdTrajectoryReceived();
+
+    int trajectory_id_;
+
+    std::vector<robot_msgs::JointTraj> joint_trajectory_vector_;
+
+    std::vector<int> joint_trajectory_id_;
+
+    void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg);
+
+    int request_trajectory_id_;
+
+    int current_trajectory_id_;
+
+    std::map<int,int> joint_trajectory_status_;
+
+    std::map<int,double>joint_trajectory_time_;
+
+    enum JointTrajectoryStatus{
+      ACTIVE,
+      DONE,
+      QUEUED,
+      DELETED,
+      FAILED,
+      CANCELED,
+      NUM_STATUS
+    };
+
+    double trajectory_wait_timeout_;
+
+  };
+
+}

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	2009-03-03 04:52:40 UTC (rev 12035)
@@ -0,0 +1,757 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Sachin Chitta <sachinc@...>
+
+#include "pr2_mechanism_controllers/whole_body_trajectory_controller.h"
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(WholeBodyTrajectoryController);
+
+
+WholeBodyTrajectoryController::WholeBodyTrajectoryController() :
+  refresh_rt_vals_(false),trajectory_type_("linear"),trajectory_wait_time_(0.0), max_update_time_(0.0)
+{
+  controller_state_publisher_ = NULL;
+}
+
+WholeBodyTrajectoryController::~WholeBodyTrajectoryController()
+{
+  // Assumes the joint controllers are owned by this class.
+  for(unsigned int i=0; i<joint_pv_controllers_.size();++i)
+    delete joint_pv_controllers_[i];
+}
+
+bool WholeBodyTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  ROS_INFO("Initializing trajectory controller");
+  int num_joints = 0;
+
+  robot_ = robot->model_;
+  mechanism::Joint *joint;
+  std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
+
+  TiXmlElement *elt = config->FirstChildElement("controller");
+  while (elt)
+  {
+    if(static_cast<std::string>(elt->Attribute("type")) == std::string("JointPVController"))
+    {
+      JointPDController * jpc = new JointPDController();
+      ROS_INFO("Joint PD Controller: %s , %s",(elt->Attribute("type")),(elt->Attribute("name")));
+
+      joint_pv_controllers_.push_back(jpc);
+      if(!jpc->initXml(robot, elt))
+        return false;
+
+      joint = (robot->getJointState(jpc->getJointName()))->joint_;
+
+      joint_type_.push_back(joint->type_);
+
+      if(joint)
+        joint_velocity_limits_.push_back(joint->velocity_limit_*velocity_scaling_factor_);
+      joint_name_.push_back(std::string(jpc->getJointName()));
+      num_joints++;
+    }
+    else if(static_cast<std::string>(elt->Attribute("type")) == std::string("BasePIDController"))
+    {
+      TiXmlElement *p = elt->FirstChildElement("pid");
+      control_toolbox::Pid pid;
+      if (p)
+        pid.initXml(p);
+      else
+        fprintf(stderr, "BasePIDController's config did not specify the default pid parameters.\n");
+      
+      base_pid_controller_.push_back(pid);
+
+      TiXmlElement *pj = elt->FirstChildElement("joint");
+      if(pj)
+      {
+        std::string joint_name = static_cast<std::string>(pj->Attribute("name"));
+        joint_name_.push_back(joint_name);        
+
+        double velocity_limit = atof(pj->Attribute("limit"));
+        joint_velocity_limits_.push_back(velocity_limit);
+
+        if(static_cast<std::string>(pj->Attribute("type")) == std::string("ROTARY_CONTINUOUS"))
+        {
+          joint_type_.push_back(mechanism::JOINT_CONTINUOUS);
+        }
+        else if(static_cast<std::string>(pj->Attribute("type")) == std::string("PRISMATIC_CONTINUOUS"))
+        {
+          joint_type_.push_back(mechanism::JOINT_PRISMATIC);
+        }
+        else
+        {
+          ROS_ERROR("Unknown type %s for base joint %s",(static_cast<std::string>(pj->Attribute("type"))).c_str(),joint_name.c_str());
+        }
+        base_joint_index_.push_back(num_joints);
+
+      }
+      current_joint_position_.push_back(0.0);
+      current_joint_velocity_.push_back(0.0);
+
+      num_joints++;
+    }
+    else if(static_cast<std::string>(elt->Attribute("type")) == std::string("BaseControllerNode"))
+    {        
+      base_controller_node_.initXml(robot,elt);
+    }
+    else
+    {
+      ROS_ERROR("Unrecognized joint controller type: %s",(static_cast<std::string>(elt->Attribute("type"))).c_str());
+    }
+    elt = elt->NextSiblingElement("controller");
+  }
+
+  ROS_INFO("WholeBodyTrajectoryController:: Done loading controllers");
+
+  elt = config->FirstChildElement("trajectory");
+
+  if(!elt)
+    ROS_WARN("No trajectory information in xml file. ");
+  else
+  {
+    trajectory_type_ = std::string(elt->Attribute("interpolation"));
+    ROS_INFO("WholeBodyTrajectoryController:: interpolation type:: %s",trajectory_type_.c_str());
+  }
+  joint_cmd_rt_.resize(num_joints);
+  joint_cmd_dot_rt_.resize(num_joints);
+
+  joint_trajectory_ = new trajectory::Trajectory((int) num_joints);
+
+  joint_trajectory_->setMaxRates(joint_velocity_limits_);
+  joint_trajectory_->setInterpolationMethod(trajectory_type_);
+
+  trajectory_point_.setDimension(num_joints);
+  dimension_ = num_joints;
+
+// Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
+  for(int j=0; j < dimension_; j++)
+    trajectory_point_.q_[j] = joint_pv_controllers_[j]->joint_state_->position_;
+  trajectory_point_.time_ = 0.0;
+  trajectory_points_vector.push_back(trajectory_point_);
+
+  updateJointValues();
+
+  for(int i=0; i < dimension_; i++)
+    trajectory_point_.q_[i] = current_joint_position_[i];
+
+  trajectory_point_.time_ = 0.0;
+  trajectory_points_vector.push_back(trajectory_point_);
+
+  joint_trajectory_->autocalc_timing_ = true;
+
+  ROS_INFO("Size of trajectory points vector : %d",trajectory_points_vector.size());
+  if(!joint_trajectory_->setTrajectory(trajectory_points_vector))
+    ROS_WARN("Trajectory not set correctly");
+
+  trajectory_start_time_ = robot_->hw_->current_time_;
+
+  last_time_ = robot_->hw_->current_time_;
+
+  return true;
+}
+
+void WholeBodyTrajectoryController::setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory)
+{
+  if(joint_trajectory.size() < 1)
+  {
+    ROS_WARN("WholeBodyTrajectoryController:: No points in trajectory");
+    return;
+  }
+
+  joint_trajectory_->setTrajectory(joint_trajectory);
+//  joint_trajectory_->write("foo.txt",0.01);
+  arm_controller_lock_.lock();
+  refresh_rt_vals_ = true;
+  arm_controller_lock_.unlock();
+}
+
+void WholeBodyTrajectoryController::getJointPosCmd(pr2_mechanism_controllers::JointPosCmd & cmd) const
+{
+}
+
+controller::JointPDController* WholeBodyTrajectoryController::getJointControllerByName(std::string name)
+{
+  for(int i=0; i< (int) joint_pv_controllers_.size(); i++)
+  {
+    if(joint_pv_controllers_[i]->getJointName() == name)
+    {
+      return joint_pv_controllers_[i];
+    }
+  }
+  return NULL;
+}
+
+int WholeBodyTrajectoryController::getJointControllerPosByName(std::string name)
+{
+  for(int i=0; i< (int) joint_pv_controllers_.size(); i++)
+  {
+    if(joint_pv_controllers_[i]->getJointName() == name)
+    {
+      return i;
+    }
+  }
+  return -1;
+}
+
+void WholeBodyTrajectoryController::update(void)
+{
+
+#ifdef PUBLISH_MAX_TIME
+  double start_time = realtime_gettime();
+#endif
+
+  double sample_time(0.0);
+
+  current_time_ = robot_->hw_->current_time_;
+
+  if(refresh_rt_vals_)
+  {
+    trajectory_start_time_ = robot_->hw_->current_time_;
+    trajectory_wait_time_ = 0.0;
+    trajectory_end_time_ = joint_trajectory_->getTotalTime()+trajectory_start_time_;
+    refresh_rt_vals_ = false;
+    trajectory_done_ = false;
+  }
+  if(arm_controller_lock_.try_lock())
+  {
+    sample_time = robot_->hw_->current_time_ - trajectory_start_time_;
+    joint_trajectory_->sample(trajectory_point_,sample_time);
+
+//    ROS_INFO("sample_time: %f", sample_time);
+    for(unsigned int i=0; i < joint_cmd_rt_.size(); ++i)
+    {
+      joint_cmd_rt_[i] = trajectory_point_.q_[i];
+      joint_cmd_dot_rt_[i] = trajectory_point_.qdot_[i];
+//      cout << " " << joint_cmd_rt_[i];
+    }
+//    cout << endl;
+    arm_controller_lock_.unlock();
+
+    if(robot_->hw_->current_time_ >= trajectory_end_time_ && trajectory_done_ == false)
+    {
+      trajectory_wait_time_ = robot_->hw_->current_time_ - trajectory_end_time_;
+      if(reachedGoalPosition(joint_cmd_rt_))
+      {
+        trajectory_wait_time_ = 0.0;
+        trajectory_done_= true;
+      }
+    }
+  }
+
+  for(unsigned int i=0;i<joint_pv_controllers_.size();++i)
+    joint_pv_controllers_[i]->setCommand(joint_cmd_rt_[i],joint_cmd_dot_rt_[i]);
+
+  updateJointControllers();
+
+  updateBaseController(current_time_);
+
+#ifdef PUBLISH_MAX_TIME
+  double end_time = realtime_gettime();
+  max_update_time_ = std::max(max_update_time_,end_time-start_time);
+
+  if (controller_state_publisher_->trylock())
+  {
+    controller_state_publisher_->msg_.update_time = end_time - start_time; 
+    controller_state_publisher_->msg_.max_update_time = max_update_time_; 
+    controller_state_publisher_->unlockAndPublish();      
+  }
+#endif
+  last_time_ = current_time_;
+}
+
+void WholeBodyTrajectoryController::updateBaseController(double time)
+{
+  double cmd[3] = {0.0,0.0,0.0};
+  for(unsigned int i=0; i< base_pid_controller_.size(); i++)
+  {
+    int joint_index = base_joint_index_[i];
+    double error(0.0), error_dot(0.0);
+    if(joint_type_[joint_index] == mechanism::JOINT_CONTINUOUS)
+    {
+      error = angles::shortest_angular_distance(joint_cmd_rt_[joint_index], current_joint_position_[joint_index]);
+    }
+    else //prismatic
+    {
+      error = current_joint_position_[joint_index] - joint_cmd_rt_[joint_index];
+    }
+    error_dot = current_joint_velocity_[joint_index] - joint_cmd_dot_rt_[joint_index];      
+    cmd[i] = base_pid_controller_[i].updatePid(error, error_dot, time - last_time_);
+    cmd[i] += joint_cmd_dot_rt_[joint_index];
+  }
+
+  base_controller_node_.setCommand(cmd[0],cmd[1],cmd[2]);
+}
+
+
+bool WholeBodyTrajectoryController::reachedGoalPosition(std::vector<double> joint_cmd)
+{
+  bool return_val = true;
+  double error(0.0);
+  for(unsigned int i=0;i< joint_pv_controllers_.size();++i)
+  {
+    if(joint_type_[i] == mechanism::JOINT_CONTINUOUS || joint_type_[i] == mechanism::JOINT_ROTARY)
+    {
+      error = fabs(angles::shortest_angular_distance(joint_cmd_rt_[i], current_joint_position_[i]));
+    }
+    else //prismatic
+    {
+      error = fabs(current_joint_position_[i] - joint_cmd_rt_[i]);
+    }
+    return_val = return_val && (error <= goal_reached_threshold_[i]);
+  }
+  for(unsigned int i=0; i< base_pid_controller_.size(); i++)
+  {
+    int joint_index = base_joint_index_[i];
+    double error(0.0);
+    if(joint_type_[joint_index] == mechanism::JOINT_CONTINUOUS)
+    {
+      error = fabs(angles::shortest_angular_distance(joint_cmd_rt_[joint_index], current_joint_position_[joint_index]));
+    }
+    else //prismatic
+    {
+      error = fabs(current_joint_position_[joint_index] - joint_cmd_rt_[joint_index]);
+    }
+    return_val = return_val && (error <= goal_reached_threshold_[i]);
+  }
+  return return_val;
+}
+
+void WholeBodyTrajectoryController::updateJointControllers(void)
+{
+  for(unsigned int i=0;i<joint_pv_controllers_.size();++i)
+    joint_pv_controllers_[i]->update();
+
+  base_controller_node_.update();
+}
+
+
+void WholeBodyTrajectoryController::updateJointValues()
+{
+  // Grab the current odometric position
+  double q[3], qdot[3];
+
+  for(unsigned int i=0; i < joint_pv_controllers_.size();++i){
+    current_joint_position_[i] = joint_pv_controllers_[i]->joint_state_->position_;
+    current_joint_velocity_[i] = joint_pv_controllers_[i]->joint_state_->velocity_;
+  }
+
+  base_controller_node_.getOdometry(q[0], q[1], q[2], qdot[0], qdot[1], qdot[2]) ;
+  for(unsigned int i=0;i< base_pid_controller_.size();++i){
+    int joint_index = base_joint_index_[i];
+    current_joint_position_[joint_index] = q[i];
+    current_joint_velocity_[joint_index] = qdot[i];
+  }
+}
+
+//------ Arm controller node --------
+
+ROS_REGISTER_CONTROLLER(WholeBodyTrajectoryControllerNode)
+
+  WholeBodyTrajectoryControllerNode::WholeBodyTrajectoryControllerNode()
+    : Controller(), node_(ros::Node::instance()), request_trajectory_id_(1), current_trajectory_id_(0), trajectory_wait_timeout_(10.0)
+{
+  std::cout<<"Controller node created"<<endl;
+  c_ = new WholeBodyTrajectoryController();
+}
+
+WholeBodyTrajectoryControllerNode::~WholeBodyTrajectoryControllerNode()
+{
+  /* node_->unadvertiseService(service_prefix_ + "/set_command");
+     node_->unadvertiseService(service_prefix_ + "/set_command_array");
+     node_->unadvertiseService(service_prefix_ + "/get_command");
+     node_->unadvertiseService(service_prefix_ + "/set_target");
+  */
+  node_->unadvertiseService(service_prefix_ + "/TrajectoryStart");
+  node_->unadvertiseService(service_prefix_ + "/TrajectoryQuery");
+
+  c_->controller_state_publisher_->stop();
+  delete c_->controller_state_publisher_;
+
+  if(topic_name_ptr_ && topic_name_.c_str())
+  {
+    std::cout << "unsub arm controller" << topic_name_ << std::endl;
+    node_->unsubscribe(topic_name_);
+  }
+  delete c_;
+}
+
+void WholeBodyTrajectoryControllerNode::update()
+{
+  if(c_->trajectory_done_)
+  {
+    updateTrajectoryQueue( WholeBodyTrajectoryControllerNode::DONE);
+  }
+  if(c_->trajectory_wait_time_ >= trajectory_wait_timeout_)
+  {
+    updateTrajectoryQueue( WholeBodyTrajectoryControllerNode::FAILED);
+  }
+
+  c_->update();
+}
+
+void WholeBodyTrajectoryControllerNode::updateTrajectoryQueue(int last_trajectory_finish_status)
+{
+  if(joint_trajectory_vector_.size() > 0)
+  {
+    if(current_trajectory_id_ == joint_trajectory_id_.front())
+    {
+      joint_trajectory_status_[current_trajectory_id_] = last_trajectory_finish_status;
+      joint_trajectory_time_[current_trajectory_id_] = c_->trajectory_end_time_ - c_->trajectory_start_time_;
+      joint_trajectory_vector_.erase(joint_trajectory_vector_.begin());
+      joint_trajectory_id_.erase(joint_trajectory_id_.begin());
+    }
+    if(joint_trajectory_vector_.size() > 0)
+    {
+      setTrajectoryCmdFromMsg(joint_trajectory_vector_.front());
+      current_trajectory_id_ = joint_trajectory_id_.front();
+      joint_trajectory_status_[current_trajectory_id_] = WholeBodyTrajectoryControllerNode::ACTIVE;
+    }
+    else
+    {
+      current_trajectory_id_ = 0;
+    }
+  }
+}
+
+
+bool WholeBodyTrajectoryControllerNode::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  ROS_INFO("Loading WholeBodyTrajectoryControllerNode.");
+  service_prefix_ = config->Attribute("name");
+  ROS_INFO("The service_prefix_ is %s",service_prefix_.c_str());
+
+  double scale;
+  node_->param<double>(service_prefix_ + "/velocity_scaling_factor",scale,0.25);
+  node_->param<double>(service_prefix_ + "/trajectory_wait_timeout",trajectory_wait_timeout_,10.0);
+
+  c_->velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
+
+  if(c_->initXml(robot, config))  // Parses subcontroller configuration
+  {
+/*    node_->advertiseService(service_prefix_ + "/set_command", &WholeBodyTrajectoryControllerNode::setJointPosHeadless, this);
+      node_->advertiseService(service_prefix_ + "/set_command_array", &WholeBodyTrajectoryControllerNode::setJointPosSrv, this);
+      node_->advertiseService(service_prefix_ + "/get_command", &WholeBodyTrajectoryControllerNode::getJointPosCmd, this);
+      node_->advertiseService(service_prefix_ + "/set_target", &WholeBodyTrajectoryControllerNode::setJointPosTarget, this);
+*/
+
+    node_->advertiseService(service_prefix_ + "/TrajectoryStart", &WholeBodyTrajectoryControllerNode::setJointTrajSrv, this);
+    node_->advertiseService(service_prefix_ + "/TrajectoryQuery", &WholeBodyTrajectoryControllerNode::queryJointTrajSrv, this);
+   node_->advertiseService(service_prefix_ + "/TrajectoryCancel", &WholeBodyTrajectoryControllerNode::cancelJointTrajSrv, this);
+
+    topic_name_ptr_ = config->FirstChildElement("listen_topic");
+    if(topic_name_ptr_)
+    {
+      topic_name_= topic_name_ptr_->Attribute("name");
+      if(!topic_name_.c_str())
+      {
+        std::cout<<" A listen _topic is present in the xml file but no name is specified\n";
+        return false;
+      }
+      node_->subscribe(topic_name_, traj_msg_, &WholeBodyTrajectoryControllerNode::CmdTrajectoryReceived, this, 1);
+      ROS_INFO("Listening to topic: %s",topic_name_.c_str());
+    }
+
+    getJointTrajectoryThresholds();
+
+
+    if (c_->controller_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+      delete c_->controller_state_publisher_ ;
+    c_->controller_state_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::ControllerState> (service_prefix_+"/controller_state", 1) ;
+
+    ROS_INFO("Initialize publisher");
+
+    c_->controller_state_publisher_->msg_.name = std::string(service_prefix_); 
+
+    ROS_INFO("Initialized controller");
+    return true;
+  }
+  ROS_INFO("Could not initialize controller");
+  return false;
+}
+
+void WholeBodyTrajectoryControllerNode::getJointTrajectoryThresholds()
+{
+  c_->goal_reached_threshold_.resize(c_->dimension_);
+  for(int i=0; i< c_->dimension_;i++)
+  {
+    node_->param<double>(service_prefix_ + "/" + c_->joint_name_[i] + "/goal_reached_threshold",c_->goal_reached_threshold_[i],GOAL_REACHED_THRESHOLD);
+  }
+}
+
+void WholeBodyTrajectoryControllerNode::setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg)
+{
+  std::vector<trajectory::Trajectory::TPoint> tp;
+  int msg_size = std::max<int>((int)traj_msg.get_points_size(),1);
+
+  tp.resize(msg_size+1);
+
+  //set first point in trajectory to current position of the arm
+  tp[0].setDimension((int) c_->dimension_);
+
+  for(int j=0; j < c_->dimension_; j++)
+  {
+    tp[0].q_[j] = c_->current_joint_position_[j];
+    tp[0].time_ = 0.0;
+  }
+
+  if((int)traj_msg.get_points_size() > 0)
+  {
+    if((int) traj_msg.points[0].get_positions_size() != (int) c_->dimension_)
+    {
+      ROS_WARN("Dimension of input trajectory = %d does not match number of controlled joints = %d",(int) traj_msg.points[0].get_positions_size(), (int) c_->dimension_);
+      return;
+    }
+    else
+    {
+      for(int i=0; i < (int) traj_msg.get_points_size(); i++)
+      {
+        tp[i+1].setDimension((int) c_->dimension_);
+        for(int j=0; j < (int) c_->dimension_; j++)
+        {
+          tp[i+1].q_[j] = traj_msg.points[i].positions[j];
+          tp[i+1].time_ = traj_msg.points[i].time;
+        }
+      }
+    }
+  }
+  else
+  {
+    ROS_WARN("Trajectory message has no way points");
+    //set second point in trajectory to current position of the arm
+    tp[1].setDimension((int) c_->dimension_);
+
+    for(int j=0; j < c_->dimension_; j++)
+    {
+      tp[1].q_[j] = c_->current_joint_position_[j];
+      tp[1].time_ = 0.0;
+    }
+  }
+
+  this->c_->setTrajectoryCmd(tp);
+}
+
+void WholeBodyTrajectoryControllerNode::CmdTrajectoryReceived()
+{
+
+  this->ros_lock_.lock();
+  setTrajectoryCmdFromMsg(traj_msg_);
+  this->ros_lock_.unlock();
+}
+
+
+bool WholeBodyTrajectoryControllerNode::getJointPosCmd(pr2_mechanism_controllers::GetJointPosCmd::Request &req,
+                                                 pr2_mechanism_controllers::GetJointPosCmd::Response &resp)
+{
+  pr2_mechanism_controllers::JointPosCmd cmd;
+  c_->getJointPosCmd(cmd);
+  resp.command = cmd;
+  return true;
+}
+
+
+bool WholeBodyTrajectoryControllerNode::setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
+                                                  pr2_mechanism_controllers::TrajectoryStart::Response &resp)
+{
+  addTrajectoryToQueue(req.traj, request_trajectory_id_);
+  resp.trajectoryid = request_trajectory_id_;
+
+  if(req.requesttiming)
+  {
+    trajectory::Trajectory tmp(c_->dimension_);
+    createTrajectory(req.traj,tmp);
+    std::vector<double> timestamps;
+    resp.set_timestamps_size((int)req.traj.get_points_size());
+    timestamps.resize((int)req.traj.get_points_size());
+
+    tmp.getTimeStamps(timestamps);
+
+    for(int i=0; i < (int) req.traj.get_points_size(); i++)
+    {
+      resp.timestamps[i] = timestamps[i];
+    }
+  }
+
+  request_trajectory_id_++;
+  return true;
+}
+
+bool WholeBodyTrajectoryControllerNode::queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
+                                                    pr2_mechanism_controllers::TrajectoryQuery::Response &resp)
+{
+  resp.set_jointnames_size(c_->dimension_);
+  resp.set_jointpositions_size(c_->dimension_);
+  for(int i=0; i < c_->dimension_; i++)
+  {
+    resp.jointnames[i] = c_->joint_name_[i];
+    resp.jointpositions[i] = c_->current_joint_position_[i];
+  }
+
+  if(req.trajectoryid == 0)
+  {
+    resp.trajectorytime = 0;
+    if(joint_trajectory_vector_.size() == 0)
+      resp.done = 1;
+    else
+      resp.done = 0;
+    return true;
+  }
+
+  std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
+  if(it == joint_trajectory_status_.end())
+    return false;
+  else
+    resp.done = it->second;
+
+
+  if(current_trajectory_id_ == (int)req.trajectoryid)
+  {
+    if((int) resp.done == 1)
+      resp.trajectorytime = c_->trajectory_end_time_ - c_->trajectory_start_time_;
+    else
+      resp.trajectorytime = c_->current_time_ - c_->trajectory_start_time_;
+  }
+  else
+  {
+    std::map<int, double>::const_iterator it_time = joint_trajectory_time_.find((int)req.trajectoryid);
+    if(it_time == joint_trajectory_time_.end())
+      return false;
+
+    resp.trajectorytime = it_time->second;
+  }
+  return true;
+}
+
+bool WholeBodyTrajectoryControllerNode::cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
+                                                     pr2_mechanism_controllers::TrajectoryCancel::Response &resp)
+{
+  int status = WholeBodyTrajectoryControllerNode::NUM_STATUS;
+
+  std::vector<trajectory::Trajectory::TPoint> trajectory_points_vector;
+  std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)req.trajectoryid);
+
+  if(it == joint_trajectory_status_.end())
+    return false;
+  else
+    status = it->second;
+
+  if(status == WholeBodyTrajectoryControllerNode::QUEUED)
+  {
+    deleteTrajectoryFromQueue(req.trajectoryid);
+  }
+  else if(status == WholeBodyTrajectoryControllerNode::ACTIVE)
+  {
+    updateTrajectoryQueue(CANCELED);
+    // Add two points since every good trajectory must have at least two points, otherwise its just a point :-)
+    for(int j=0; j < c_->dimension_; j++)
+      c_->trajectory_point_.q_[j] = c_->current_joint_position_[j];
+
+    c_->trajectory_point_.time_ = 0.0;
+    trajectory_points_vector.push_back(c_->trajectory_point_);
+
+    for(int i=0; i < c_->dimension_; i++)
+      c_->trajectory_point_.q_[i] =  c_->current_joint_position_[i];
+    c_->trajectory_point_.time_ = 0.0;
+    trajectory_points_vector.push_back(c_->trajectory_point_);
+    if(!c_->joint_trajectory_->setTrajectory(trajectory_points_vector))
+      ROS_WARN("Trajectory not set correctly");
+  }
+  return true;
+}
+
+int WholeBodyTrajectoryControllerNode::createTrajectory(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory)
+{
+  std::vector<trajectory::Trajectory::TPoint> tp;
+
+  if(new_traj.get_points_size() > 0)
+  {
+    if((int) new_traj.points[0].get_positions_size() != (int) c_->dimension_)
+    {
+      ROS_WARN("Dimension of input trajectory = %d does not match number of controlled joints = %d",(int) new_traj.points[0].get_positions_size(), (int) c_->dimension_);
+      return -1;
+    }
+  }
+  else
+  {
+    ROS_WARN("Trajectory message has no way points");
+    return -1;
+  }
+
+  tp.resize((int)new_traj.get_points_size());
+
+  for(int i=0; i < (int) new_traj.get_points_size(); i++)
+  {
+    tp[i].setDimension((int) c_->dimension_);
+    for(int j=0; j < (int) c_->dimension_; j++)
+    {
+      tp[i].q_[j] = new_traj.points[i].positions[j];
+      tp[i].time_ = new_traj.points[i].time;
+    }
+  }
+  return_trajectory.setMaxRates(c_->joint_velocity_limits_);
+  return_trajectory.setInterpolationMethod(c_->trajectory_type_);
+
+  if(!return_trajectory.setTrajectory(tp))
+  {
+    ROS_WARN("Trajectory not set correctly");
+    return -1;
+  }
+  return 1;
+}
+
+void WholeBodyTrajectoryControllerNode::addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id)
+{
+  joint_trajectory_vector_.push_back(new_traj);
+  joint_trajectory_id_.push_back(id);
+  joint_trajectory_status_[id] = WholeBodyTrajectoryControllerNode::QUEUED;
+  joint_trajectory_time_[id] = 0.0;
+}
+
+void WholeBodyTrajectoryControllerNode::deleteTrajectoryFromQueue(int id)
+{
+// do a linear search
+  for(int i = 0; i < (int) joint_trajectory_vector_.size(); i++)
+  {
+    if(joint_trajectory_id_[i] == id)
+    {
+      joint_trajectory_vector_.erase(joint_trajectory_vector_.begin()+i);
+      joint_trajectory_id_.erase(joint_trajectory_id_.begin()+i);
+      joint_trajectory_status_[id] = WholeBodyTrajectoryControllerNode::DELETED;
+      break;
+    }
+  }
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12162
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12162&view=rev
Author:   sachinchitta
Date:     2009-03-05 06:15:25 +0000 (Thu, 05 Mar 2009)

Log Message:
-----------
further fixes for diagnostic message forming

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-03-05 06:13:42 UTC (rev 12161)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-03-05 06:15:25 UTC (rev 12162)
@@ -238,7 +238,7 @@
 
     private:
 
-    void publishDiagnostics(realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> &publisher);
+    void publishDiagnostics();
 
     realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ;  //!< Publishes controller information
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-03-05 06:13:42 UTC (rev 12161)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-03-05 06:15:25 UTC (rev 12162)
@@ -125,6 +125,7 @@
     ROS_WARN("Trajectory not set correctly");
 
   trajectory_start_time_ = robot_->hw_->current_time_;
+  trajectory_end_time_ = trajectory_start_time_;
 
   return true;
 }
@@ -302,8 +303,9 @@
 
 
   c_->update();
-
-  publishDiagnostics(*diagnostics_publisher_);
+//  ROS_INFO("Publishing diagnostics");
+  publishDiagnostics();
+//  ROS_INFO("Published diagnostics");
 }
 
 void ArmTrajectoryControllerNode::updateTrajectoryQueue(int last_trajectory_finish_status)
@@ -380,7 +382,7 @@
   diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
 
 
-  ROS_INFO("Initialize publisher");
+  ROS_INFO("Initialized publisher");
 
   c_->controller_state_publisher_->msg_.name = std::string(service_prefix_); 
 
@@ -643,10 +645,12 @@
 }
 
 
-void ArmTrajectoryControllerNode::publishDiagnostics(realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> &publisher)
+void ArmTrajectoryControllerNode::publishDiagnostics()
 {
-  if(publisher.trylock())
+//  ROS_INFO("Starting diagnostics");
+  if(diagnostics_publisher_->trylock())
   {
+//    ROS_INFO("Started diagnostics");
     robot_msgs::JointCmd cmd;
     cmd.set_names_size(1);
     cmd.set_positions_size(1);
@@ -662,43 +666,64 @@
     status.level = 0;
     status.message = "OK";
 
+//    ROS_INFO("Diagnostics 1");
+
     for(unsigned int i=0; i < c_->joint_pd_controllers_.size(); i++)
     {
       v.label = c_->joint_pd_controllers_[i]->getJointName() + "/Position/Current";
       v.value = c_->joint_pd_controllers_[i]->joint_state_->position_;
       values.push_back(v);
 
+//      ROS_INFO("Diagnostics %d: 1",i);
+
       v.label = c_->joint_pd_controllers_[i]->getJointName() + "/Position/Command";
       c_->joint_pd_controllers_[i]->getCommand(cmd);
       v.value = cmd.positions[0];
       values.push_back(v);
+
+//      ROS_INFO("Diagnostics %d: 2",i);
+
     }
 
-    v.label = "Trajectory id: ";
+    v.label = "Trajectory id";
     v.value = current_trajectory_id_;
     values.push_back(v);
 
-    v.label = "Trajectory Status";
+//    ROS_INFO("Diagnostics 2");
+
+    v.label = "Trajectory Status:: ";
     std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)current_trajectory_id_);
     if(it == joint_trajectory_status_.end())
+    {
       v.label += "UNKNOWN";
+      v.value = -1;
+    }
     else
+    {
       v.label += JointTrajectoryStatusString[it->second];
-
+      v.value = it->second;
+    }
     values.push_back(v);
 
+//    ROS_INFO("Diagnostics 3");
+
     v.label = "Trajectory Current Time";
     v.value = c_->current_time_-c_->trajectory_start_time_;
     values.push_back(v);
 
+//    ROS_INFO("Diagnostics 4");
+
     v.label = "Trajectory Expected End Time (computed)";
     v.value = c_->trajectory_end_time_-c_->trajectory_start_time_;
     values.push_back(v);
 
+//    ROS_INFO("Diagnostics 5");
+
     status.set_values_vec(values);
     status.set_strings_vec(strings);
     statuses.push_back(status);
-    publisher.msg_.set_status_vec(statuses);
-    publisher.unlockAndPublish();
+    diagnostics_publisher_->msg_.set_status_vec(statuses);
+//    ROS_INFO("Set diagnostics info");
+    diagnostics_publisher_->unlockAndPublish();
   }
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12171
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12171&view=rev
Author:   sachinchitta
Date:     2009-03-05 08:40:42 +0000 (Thu, 05 Mar 2009)

Log Message:
-----------
arm trajectory controller now broadcasts a diagnostic message with information about joints being controller, actual and commanded positions and errors, trajectory ids, current time, expected completion time and trajectory status

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-03-05 08:25:23 UTC (rev 12170)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/arm_trajectory_controller.h	2009-03-05 08:40:42 UTC (rev 12171)
@@ -306,6 +306,10 @@
 
     double trajectory_wait_timeout_;
 
+    double last_diagnostics_publish_time_;
+
+    double diagnostics_publish_delta_time_;
+
   };
 
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-03-05 08:25:23 UTC (rev 12170)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_trajectory_controller.cpp	2009-03-05 08:40:42 UTC (rev 12171)
@@ -304,7 +304,11 @@
 
   c_->update();
 //  ROS_INFO("Publishing diagnostics");
-  publishDiagnostics();
+  if((c_->current_time_ - last_diagnostics_publish_time_) > diagnostics_publish_delta_time_)
+  {
+    publishDiagnostics();
+    last_diagnostics_publish_time_ = c_->current_time_;
+  }
 //  ROS_INFO("Published diagnostics");
 }
 
@@ -381,6 +385,8 @@
     delete diagnostics_publisher_ ;
   diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
 
+  last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
+  node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
 
   ROS_INFO("Initialized publisher");
 
@@ -670,7 +676,7 @@
 
     for(unsigned int i=0; i < c_->joint_pd_controllers_.size(); i++)
     {
-      v.label = c_->joint_pd_controllers_[i]->getJointName() + "/Position/Current";
+      v.label = c_->joint_pd_controllers_[i]->getJointName() + "/Position/Actual";
       v.value = c_->joint_pd_controllers_[i]->joint_state_->position_;
       values.push_back(v);
 
@@ -681,6 +687,10 @@
       v.value = cmd.positions[0];
       values.push_back(v);
 
+      v.label = c_->joint_pd_controllers_[i]->getJointName() + "/Position/Error (Command-Actual)";
+      v.value = cmd.positions[0] - c_->joint_pd_controllers_[i]->joint_state_->position_;
+      values.push_back(v);
+
 //      ROS_INFO("Diagnostics %d: 2",i);
 
     }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12172
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12172&view=rev
Author:   sachinchitta
Date:     2009-03-05 09:29:20 +0000 (Thu, 05 Mar 2009)

Log Message:
-----------
Whole body trajectory controller now has diagnostic information

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	2009-03-05 08:40:42 UTC (rev 12171)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	2009-03-05 09:29:20 UTC (rev 12172)
@@ -58,6 +58,7 @@
 
 #include <robot_msgs/JointTraj.h>
 #include <robot_msgs/JointTrajPoint.h>
+#include <robot_msgs/DiagnosticMessage.h>
 
 #include <pr2_mechanism_controllers/TrajectoryStart.h>
 #include <pr2_mechanism_controllers/TrajectoryQuery.h>
@@ -79,9 +80,10 @@
 namespace controller
 {
 
+  const std::string JointTrajectoryStatusString[7] = {"0 - ACTIVE","1 - DONE","2 - QUEUED","3 - DELETED","4 - FAILED","5 - CANCELED","6 - NUM_STATUS"};
+
   #define GOAL_REACHED_THRESHOLD 0.01
 
-
     // comment this out if the controller is not supposed to publish its own max execution time
   #define PUBLISH_MAX_TIME
 
@@ -257,6 +259,10 @@
 
     private:
 
+    void publishDiagnostics();
+
+    realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ;  //!< Publishes controller information
+
     /*!
      * \brief mutex lock for setting and getting ros messages
      */
@@ -321,6 +327,10 @@
 
     double trajectory_wait_timeout_;
 
+    double last_diagnostics_publish_time_;
+
+    double diagnostics_publish_delta_time_;
+
   };
 
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	2009-03-05 08:40:42 UTC (rev 12171)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	2009-03-05 09:29:20 UTC (rev 12172)
@@ -397,6 +397,7 @@
 {
   std::cout<<"Controller node created"<<endl;
   c_ = new WholeBodyTrajectoryController();
+  diagnostics_publisher_ = NULL;
 }
 
 WholeBodyTrajectoryControllerNode::~WholeBodyTrajectoryControllerNode()
@@ -412,6 +413,9 @@
   c_->controller_state_publisher_->stop();
   delete c_->controller_state_publisher_;
 
+  diagnostics_publisher_->stop();
+  delete diagnostics_publisher_;
+
   if(topic_name_ptr_ && topic_name_.c_str())
   {
     std::cout << "unsub arm controller" << topic_name_ << std::endl;
@@ -434,6 +438,13 @@
   }
 
   c_->update();
+
+  if((c_->current_time_ - last_diagnostics_publish_time_) > diagnostics_publish_delta_time_)
+  {
+    publishDiagnostics();
+    last_diagnostics_publish_time_ = c_->current_time_;
+  }
+
 }
 
 void WholeBodyTrajectoryControllerNode::updateTrajectoryQueue(int last_trajectory_finish_status)
@@ -505,6 +516,13 @@
       delete c_->controller_state_publisher_ ;
     c_->controller_state_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::ControllerState> (service_prefix_+"/controller_state", 1) ;
 
+  if (diagnostics_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
+    delete diagnostics_publisher_ ;
+  diagnostics_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage> ("/diagnostics", 2) ;
+
+  last_diagnostics_publish_time_ = c_->robot_->hw_->current_time_;
+  node_->param<double>(service_prefix_ + "/diagnostics_publish_delta_time",diagnostics_publish_delta_time_,0.05);
+
     ROS_INFO("Initialize publisher");
 
     c_->controller_state_publisher_->msg_.name = std::string(service_prefix_); 
@@ -767,3 +785,112 @@
     }
   }
 }
+
+
+void WholeBodyTrajectoryControllerNode::publishDiagnostics()
+{
+//  ROS_INFO("Starting diagnostics");
+  if(diagnostics_publisher_->trylock())
+  {
+//    ROS_INFO("Started diagnostics");
+    robot_msgs::JointCmd cmd;
+    cmd.set_names_size(1);
+    cmd.set_positions_size(1);
+    cmd.set_velocity_size(1);
+
+    vector<robot_msgs::DiagnosticStatus> statuses;
+    vector<robot_msgs::DiagnosticValue> values;
+    vector<robot_msgs::DiagnosticString> strings;
+    robot_msgs::DiagnosticStatus status;
+    robot_msgs::DiagnosticValue v;
+
+    status.name = "Whole Body Trajectory Controller";
+    status.level = 0;
+    status.message = "OK";
+
+//    ROS_INFO("Diagnostics 1");
+
+    for(unsigned int i=0; i < c_->joint_pv_controllers_.size(); i++)
+    {
+      v.label = c_->joint_pv_controllers_[i]->getJointName() + "/Position/Actual";
+      v.value = c_->joint_pv_controllers_[i]->joint_state_->position_;
+      values.push_back(v);
+
+//      ROS_INFO("Diagnostics %d: 1",i);
+
+      v.label = c_->joint_pv_controllers_[i]->getJointName() + "/Position/Command";
+      c_->joint_pv_controllers_[i]->getCommand(cmd);
+      v.value = cmd.positions[0];
+      values.push_back(v);
+
+      v.label = c_->joint_pv_controllers_[i]->getJointName() + "/Position/Error (Command-Actual)";
+      v.value = cmd.positions[0] - c_->joint_pv_controllers_[i]->joint_state_->position_;
+      values.push_back(v);
+
+//      ROS_INFO("Diagnostics %d: 2",i);
+
+    }
+
+
+    for(unsigned int i=0; i < c_->base_pid_controller_.size(); i++)
+    {
+      int joint_index = c_->base_joint_index_[i];
+      v.label = c_->joint_name_[joint_index] + "/Position/Actual";
+      v.value = c_->current_joint_position_[joint_index];
+      values.push_back(v);
+
+//      ROS_INFO("Diagnostics %d: 1",i);
+
+      v.label = c_->joint_name_[joint_index] + "/Position/Command";
+      v.value = c_->joint_cmd_rt_[joint_index];
+      values.push_back(v);
+
+      v.label = c_->joint_name_[joint_index] + "/Position/Error (Command-Actual)";
+      v.value = c_->joint_cmd_rt_[joint_index] - c_->current_joint_position_[joint_index];
+      values.push_back(v);
+
+//      ROS_INFO("Diagnostics %d: 2",i);
+    }
+
+    v.label = "Trajectory id";
+    v.value = current_trajectory_id_;
+    values.push_back(v);
+
+//    ROS_INFO("Diagnostics 2");
+
+    v.label = "Trajectory Status:: ";
+    std::map<int, int>::const_iterator it = joint_trajectory_status_.find((int)current_trajectory_id_);
+    if(it == joint_trajectory_status_.end())
+    {
+      v.label += "UNKNOWN";
+      v.value = -1;
+    }
+    else
+    {
+      v.label += JointTrajectoryStatusString[it->second];
+      v.value = it->second;
+    }
+    values.push_back(v);
+
+//    ROS_INFO("Diagnostics 3");
+
+    v.label = "Trajectory Current Time";
+    v.value = c_->current_time_-c_->trajectory_start_time_;
+    values.push_back(v);
+
+//    ROS_INFO("Diagnostics 4");
+
+    v.label = "Trajectory Expected End Time (computed)";
+    v.value = c_->trajectory_end_time_-c_->trajectory_start_time_;
+    values.push_back(v);
+
+//    ROS_INFO("Diagnostics 5");
+
+    status.set_values_vec(values);
+    status.set_strings_vec(strings);
+    statuses.push_back(status);
+    diagnostics_publisher_->msg_.set_status_vec(statuses);
+//    ROS_INFO("Set diagnostics info");
+    diagnostics_publisher_->unlockAndPublish();
+  }
+}

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp	2009-03-05 08:40:42 UTC (rev 12171)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_whole_body_controller.cpp	2009-03-05 09:29:20 UTC (rev 12172)
@@ -72,7 +72,7 @@
   cmd.points[0].positions[4] = 0.0;
   cmd.points[0].positions[5] = 0.0;
   cmd.points[0].positions[6] = 0.0;
-  cmd.points[0].positions[7] = -0.0;
+  cmd.points[0].positions[7] = -2.0;
   cmd.points[0].positions[8] = 0.0;
   cmd.points[0].positions[9] = -0.0;
   cmd.points[0].time = 0.0;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12255
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12255&view=rev
Author:   vijaypradeep
Date:     2009-03-06 21:19:55 +0000 (Fri, 06 Mar 2009)

Log Message:
-----------
Fixed laser_traj_controller segfault. Controller now holds initial position until a traj command is sent

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-06 21:18:40 UTC (rev 12254)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-06 21:19:55 UTC (rev 12255)
@@ -12,7 +12,7 @@
   src/base_controller_pos.cpp
   src/base_position_controller.cpp
   src/laser_scanner_controller.cpp
-#Removed while filters are unstable  src/laser_scanner_traj_controller.cpp 
+  src/laser_scanner_traj_controller.cpp 
   src/laser_scanner_velocity_controller.cpp
   src/arm_position_controller.cpp
   src/arm_velocity_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-03-06 21:18:40 UTC (rev 12254)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-03-06 21:19:55 UTC (rev 12255)
@@ -130,6 +130,17 @@
   if(max_acc_elem->QueryDoubleAttribute("value", &max_acc_) != TIXML_SUCCESS )
     return false ;
 
+  // Set to hold the current position
+
+  pr2_mechanism_controllers::PeriodicCmd cmd ;
+  cmd.profile = "linear" ;
+  cmd.period = 1.0 ;
+  cmd.amplitude = 0.0 ;
+  cmd.offset = joint_state_->position_ ;
+
+  setPeriodicCmd(cmd) ;
+
+
   return true ;
 }
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12349
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12349&view=rev
Author:   meeussen
Date:     2009-03-10 18:18:25 +0000 (Tue, 10 Mar 2009)

Log Message:
-----------
add extra controllers for caster that can work without wheel encoders. This controllers should be removed once the wheel encoders arrive

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-10 18:16:51 UTC (rev 12348)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-10 18:18:25 UTC (rev 12349)
@@ -20,7 +20,9 @@
   src/head_pan_tilt_controller.cpp
   src/head_servoing_controller.cpp
   src/caster_controller.cpp
+  src/caster_controller_effort.cpp
   src/caster_calibration_controller.cpp
+  src/caster_calibration_controller_effort.cpp
   src/wrist_calibration_controller.cpp
   src/gripper_calibration_controller.cpp
   src/arm_trajectory_controller.cpp

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h (from rev 12243, pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller_effort.h	2009-03-10 18:18:25 UTC (rev 12349)
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#ifndef CASTER_CALIBRATION_CONTROLLER_EFFORT_H
+#define CASTER_CALIBRATION_CONTROLLER_EFFORT_H
+
+#include "pr2_mechanism_controllers/caster_controller_effort.h"
+#include "realtime_tools/realtime_publisher.h"
+#include "std_msgs/Empty.h"
+#include <robot_mechanism_controllers/CalibrateJoint.h>
+
+namespace controller {
+
+class CasterCalibrationControllerEffort : public Controller
+{
+public:
+  CasterCalibrationControllerEffort();
+  ~CasterCalibrationControllerEffort();
+
+  virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+  virtual void update();
+
+  bool calibrated() { return state_ == CALIBRATED; }
+  void beginCalibration()
+  {
+    if (state_ == INITIALIZED)
+      state_ = BEGINNING;
+  }
+
+protected:
+
+  enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
+  int state_;
+
+  double search_velocity_;
+  bool original_switch_state_;
+
+  Actuator *actuator_;
+  mechanism::JointState *joint_;
+  mechanism::Transmission *transmission_;
+
+  controller::CasterControllerEffort cc_;
+};
+
+
+class CasterCalibrationControllerEffortNode : public Controller
+{
+public:
+  CasterCalibrationControllerEffortNode();
+  ~CasterCalibrationControllerEffortNode();
+
+  void update();
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  bool calibrateCommand(robot_mechanism_controllers::CalibrateJoint::Request &req,
+                        robot_mechanism_controllers::CalibrateJoint::Response &resp);
+
+private:
+  mechanism::RobotState *robot_;
+  CasterCalibrationControllerEffort c_;
+  AdvertisedServiceGuard guard_calibrate_;
+
+  double last_publish_time_;
+  realtime_tools::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
+};
+
+}
+
+#endif

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h (from rev 12243, pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller.h)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_controller_effort.h	2009-03-10 18:18:25 UTC (rev 12349)
@@ -0,0 +1,116 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Wim Meeussen
+
+ Example config:
+
+ <controller type="CasterControllerEffort" name="a_caster">
+   <joints caster="caster_joint" wheel_l="wheel_left_joint" wheel_r="wheel_right_joint">
+   <caster_pid p="5.0" i="0.0" d="0.0" iClamp="0.0" />
+   <wheel_pid p="4.0" i="0.0" d="0.0" iClamp="0.0" />
+ </controller>
+ */
+#ifndef CASTER_CONTROLLER_EFFORT_H
+#define CASTER_CONTROLLER_EFFORT_H
+
+#include "mechanism_model/controller.h"
+#include "mechanism_model/robot.h"
+#include "control_toolbox/pid.h"
+#include "robot_mechanism_controllers/joint_velocity_controller.h"
+#include "robot_mechanism_controllers/joint_effort_controller.h"
+#include "misc_utils/subscription_guard.h"
+#include "std_msgs/Float64.h"
+
+namespace controller {
+
+class CasterControllerEffort : public Controller
+{
+public:
+  const static double WHEEL_RADIUS;
+  const static double WHEEL_OFFSET;
+  const static double VEL_TO_EFF;
+
+  CasterControllerEffort();
+  ~CasterControllerEffort();
+
+  bool init(mechanism::RobotState *robot_state,
+            const std::string &caster_joint,
+            const std::string &wheel_l_joint, const std::string &wheel_r_joint,
+            const control_toolbox::Pid &caster_pid);
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  void update();
+
+  double steer_velocity_;
+  double drive_effort_;
+
+  double getSteerPosition() { return caster_->position_; }
+  double getSteerVelocity() { return caster_->velocity_; }
+
+  mechanism::JointState *caster_;
+
+private:
+  JointVelocityController caster_vel_;
+  JointEffortController wheel_l_eff_, wheel_r_eff_;
+};
+
+
+/*
+ * Listens on /name/steer_velocity and /name/drive_velocity
+ */
+
+class CasterControllerEffortNode : public Controller
+{
+public:
+  CasterControllerEffortNode();
+  ~CasterControllerEffortNode();
+
+  bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+  void update();
+
+  void setSteerVelocity() {
+    c_.steer_velocity_ = steer_velocity_msg_.data;
+  }
+  void setDriveEffort() {
+    c_.drive_effort_ = drive_effort_msg_.data;
+  }
+
+private:
+  CasterControllerEffort c_;
+
+  SubscriptionGuard guard_steer_velocity_, guard_drive_effort_;
+  std_msgs::Float64 steer_velocity_msg_, drive_effort_msg_;
+};
+
+}
+
+#endif

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp (from rev 12243, pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller_effort.cpp	2009-03-10 18:18:25 UTC (rev 12349)
@@ -0,0 +1,222 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#include "pr2_mechanism_controllers/caster_calibration_controller_effort.h"
+
+namespace controller {
+
+CasterCalibrationControllerEffort::CasterCalibrationControllerEffort()
+  : state_(INITIALIZED)
+{
+}
+
+CasterCalibrationControllerEffort::~CasterCalibrationControllerEffort()
+{
+}
+
+bool CasterCalibrationControllerEffort::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  // This method is gross and ugly and should change (and the xml
+  // config format along with it)
+  assert(robot);
+  assert(config);
+
+  TiXmlElement *cal = config->FirstChildElement("calibrate");
+  if (!cal)
+  {
+    std::cerr<<"CasterCalibrationControllerEffort was not given calibration parameters"<<std::endl;
+    return false;
+  }
+
+  if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
+  {
+    std::cerr<<"Velocity value was not specified\n";
+    return false;
+  }
+
+  const char *joint_name = cal->Attribute("joint");
+  joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+  if (!joint_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationControllerEffort could not find joint \"%s\"\n",
+            joint_name);
+    return false;
+  }
+
+  const char *actuator_name = cal->Attribute("actuator");
+  actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+  if (!actuator_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationControllerEffort could not find actuator \"%s\"\n",
+            actuator_name);
+    return false;
+  }
+
+  const char *transmission_name = cal->Attribute("transmission");
+  transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+  if (!transmission_)
+  {
+    fprintf(stderr, "Error: CasterCalibrationControllerEffort could not find transmission \"%s\"\n",
+            transmission_name);
+    return false;
+  }
+
+  if (!cc_.initXml(robot, config))
+    return false;
+
+  return true;
+}
+
+void CasterCalibrationControllerEffort::update()
+{
+  assert(joint_);
+  assert(actuator_);
+
+  switch(state_)
+  {
+  case INITIALIZED:
+    cc_.steer_velocity_ = 0.0;
+    cc_.drive_effort_ = 0.0;
+    state_ = BEGINNING;
+    break;
+  case BEGINNING:
+    original_switch_state_ = actuator_->state_.calibration_reading_;
+    cc_.steer_velocity_ = (original_switch_state_ ? -search_velocity_ : search_velocity_);
+    state_ = MOVING;
+    break;
+  case MOVING: {
+    bool switch_state_ = actuator_->state_.calibration_reading_;
+    if (switch_state_ != original_switch_state_)
+    {
+      Actuator a;
+      mechanism::JointState j;
+      std::vector<Actuator*> fake_a;
+      std::vector<mechanism::JointState*> fake_j;
+      fake_a.push_back(&a);
+      fake_j.push_back(&j);
+
+      // Where was the joint when the optical switch triggered?
+      if (switch_state_ == true)
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
+      else
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_falling_edge_;
+      transmission_->propagatePosition(fake_a, fake_j);
+
+      // What is the actuator position at the joint's zero?
+      fake_j[0]->position_ = fake_j[0]->position_ - joint_->joint_->reference_position_;
+      transmission_->propagatePositionBackwards(fake_j, fake_a);
+
+      actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+      joint_->calibrated_ = true;
+
+      state_ = CALIBRATED;
+    }
+    break;
+  }
+  case CALIBRATED:
+    cc_.steer_velocity_ = 0.0;
+    break;
+  }
+
+  if (state_ != CALIBRATED)
+    cc_.update();
+}
+
+
+ROS_REGISTER_CONTROLLER(CasterCalibrationControllerEffortNode)
+
+CasterCalibrationControllerEffortNode::CasterCalibrationControllerEffortNode()
+: last_publish_time_(0), pub_calibrated_(NULL)
+{
+}
+
+CasterCalibrationControllerEffortNode::~CasterCalibrationControllerEffortNode()
+{
+  if (pub_calibrated_)
+    delete pub_calibrated_;
+}
+
+void CasterCalibrationControllerEffortNode::update()
+{
+  c_.update();
+
+  if (c_.calibrated())
+  {
+    if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+    {
+      assert(pub_calibrated_);
+      if (pub_calibrated_->trylock())
+      {
+        last_publish_time_ = robot_->hw_->current_time_;
+        pub_calibrated_->unlockAndPublish();
+      }
+    }
+  }
+}
+
+
+  bool CasterCalibrationControllerEffortNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::Request &req, robot_mechanism_controllers::CalibrateJoint::Response &resp)
+{
+  c_.beginCalibration();
+  ros::Duration d=ros::Duration(0,1000000);
+  while(!c_.calibrated())
+    d.sleep();
+  resp.offset = 0;
+  return true;
+}
+
+bool CasterCalibrationControllerEffortNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  assert(robot);
+  robot_ = robot;
+  ros::Node *node = ros::Node::instance();
+
+  std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
+  if (topic == "")
+  {
+    fprintf(stderr, "No topic given to CasterCalibrationControllerEffort\n");
+    return false;
+  }
+
+  if (!c_.initXml(robot, config))
+    return false;
+  node->advertiseService(topic + "/calibrate", &CasterCalibrationControllerEffortNode::calibrateCommand, this);
+  guard_calibrate_.set(topic + "/calibrate");
+
+  pub_calibrated_ = new realtime_tools::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
+  return true;
+}
+
+
+}

Copied: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp (from rev 12243, pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller.cpp)
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_controller_effort.cpp	2009-03-10 18:18:25 UTC (rev 12349)
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Author: Wim Meeussen
+ */
+
+#include "pr2_mechanism_controllers/caster_controller_effort.h"
+
+namespace controller {
+
+const double CasterControllerEffort::WHEEL_RADIUS = 0.079;
+const double CasterControllerEffort::WHEEL_OFFSET = 0.049;
+const double CasterControllerEffort::VEL_TO_EFF   = 10.0;
+
+CasterControllerEffort::CasterControllerEffort()
+  : steer_velocity_(0), drive_effort_(0)
+{
+}
+
+CasterControllerEffort::~CasterControllerEffort()
+{
+}
+
+bool CasterControllerEffort::init(
+  mechanism::RobotState *robot,
+  const std::string &caster_joint,
+  const std::string &wheel_l_joint, const std::string &wheel_r_joint,
+  const control_toolbox::Pid &caster_pid)
+{
+  caster_ = robot->getJointState(caster_joint);
+  if (!caster_)
+  {
+    fprintf(stderr, "Error: Caster joint \"%s\" does not exist\n", caster_joint.c_str());
+    return false;
+  }
+
+  if (!caster_vel_.init(robot, caster_joint, caster_pid))
+    return false;
+  if (!wheel_l_eff_.init(robot, wheel_l_joint))
+    return false;
+  if (!wheel_r_eff_.init(robot, wheel_r_joint))
+    return false;
+
+  return true;
+}
+
+bool CasterControllerEffort::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  TiXmlElement *jel = config->FirstChildElement("joints");
+  if (!jel)
+  {
+    fprintf(stderr, "Error: CasterControllerEffort could not find joints element\n");
+    return false;
+  }
+
+  const char *caster_joint = jel->Attribute("caster");
+  const char *wheel_l_joint = jel->Attribute("wheel_l");
+  const char *wheel_r_joint = jel->Attribute("wheel_r");
+  if (!caster_joint)
+  {
+    fprintf(stderr, "Error: CasterControllerEffort was not given a caster joint\n");
+    return false;
+  }
+  if (!wheel_l_joint)
+  {
+    fprintf(stderr, "Error: CasterControllerEffort was not given a wheel_l joint\n");
+    return false;
+  }
+  if (!wheel_r_joint)
+  {
+    fprintf(stderr, "Error: CasterControllerEffort was not given a wheel_r joint\n");
+    return false;
+  }
+
+  control_toolbox::Pid caster_pid;
+  TiXmlElement *cpel = config->FirstChildElement("caster_pid");
+  if (!cpel)
+  {
+    fprintf(stderr, "Error: CasterControllerEffort was not given a caster_pid element\n");
+    return false;
+  }
+  if (!caster_pid.initXml(cpel))
+    return false;
+
+  return init(robot, caster_joint, wheel_l_joint, wheel_r_joint, caster_pid);
+}
+
+void CasterControllerEffort::update()
+{
+  caster_vel_.setCommand(steer_velocity_);
+
+  double wd = drive_effort_;  // Wheel torque due to driving
+  double ws = VEL_TO_EFF * (WHEEL_RADIUS / WHEEL_OFFSET) * steer_velocity_;  // Wheel torque due to steering
+  wheel_r_eff_.setCommand(wd + ws);
+  wheel_l_eff_.setCommand(wd - ws);
+
+  caster_vel_.update();
+  wheel_l_eff_.update();
+  wheel_r_eff_.update();
+}
+
+
+
+ROS_REGISTER_CONTROLLER(CasterControllerEffortNode);
+
+CasterControllerEffortNode::CasterControllerEffortNode()
+{
+}
+
+CasterControllerEffortNode::~CasterControllerEffortNode()
+{
+}
+
+bool CasterControllerEffortNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
+{
+  ros::Node *node = ros::Node::instance();
+  assert(node);
+
+  std::string name = config->Attribute("name") ? config->Attribute("name") : "";
+  if (name == "")
+  {
+    fprintf(stderr, "Error: No name given for CasterControllerEffortNode\n");
+    return false;
+  }
+
+  if (!c_.initXml(robot, config))
+    return false;
+
+  node->subscribe(name + "/steer_velocity", steer_velocity_msg_,
+                  &CasterControllerEffortNode::setSteerVelocity, this, 2);
+  guard_steer_velocity_.set(name + "/steer_velocity");
+  node->subscribe(name + "/drive_effort", drive_effort_msg_,
+                  &CasterControllerEffortNode::setDriveEffort, this, 2);
+  guard_drive_effort_.set(name + "/drive_velocity");
+
+  return true;
+}
+
+void CasterControllerEffortNode::update()
+{
+  if (!c_.caster_->calibrated_)
+    return;
+  c_.update();
+}
+
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12794
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12794&view=rev
Author:   sachinchitta
Date:     2009-03-20 23:39:09 +0000 (Fri, 20 Mar 2009)

Log Message:
-----------
updated trajectory controller now includes a watchdot to shut off if no commands are coming in or if errors build up too high

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-20 23:15:33 UTC (rev 12793)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/CMakeLists.txt	2009-03-20 23:39:09 UTC (rev 12794)
@@ -43,7 +43,7 @@
 rospack_declare_test(testOdom)
 target_link_libraries(testOdom pr2_mechanism_controllers)
 
-rospack_add_executable(testTraj EXCLUDE_FROM_ALL test/test_arm_trajectory_controller.cpp)
+rospack_add_executable(testTraj test/test_arm_trajectory_controller.cpp)
 rospack_declare_test(testTraj)
 target_link_libraries(testTraj pr2_mechanism_controllers)
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	2009-03-20 23:15:33 UTC (rev 12793)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/whole_body_trajectory_controller.h	2009-03-20 23:39:09 UTC (rev 12794)
@@ -83,7 +83,7 @@
   const std::string JointTrajectoryStatusString[7] = {"0 - ACTIVE","1 - DONE","2 - QUEUED","3 - DELETED","4 - FAILED","5 - CANCELED","6 - NUM_STATUS"};
 
   #define GOAL_REACHED_THRESHOLD 0.01
-
+  #define MAX_ALLOWABLE_JOINT_ERROR_THRESHOLD 0.2
     // comment this out if the controller is not supposed to publish its own max execution time
   #define PUBLISH_MAX_TIME
 
@@ -143,6 +143,10 @@
 
     std::vector<double> current_joint_velocity_;
 
+    std::vector<double> joint_errors_;
+
+    std::vector<double> max_allowable_joint_errors_;
+
     controller::BaseControllerNode base_controller_node_;
 
     std::vector<JointPDController *> joint_pv_controllers_;
@@ -165,8 +169,16 @@
 
     bool reachedGoalPosition(std::vector<double> joint_cmd);
 
+    bool errorsWithinThreshold();
+
+    void computeJointErrors();
+
     int getJointControllerPosByName(std::string name);
 
+    void checkWatchDog(double current_time);
+
+    void stopMotion();
+
     // Indicates if goals_ and error_margins_ should be copied into goals_rt_ and error_margins_rt_
     bool refresh_rt_vals_;
 
@@ -195,6 +207,12 @@
     realtime_tools::RealtimePublisher <robot_msgs::ControllerState>* controller_state_publisher_ ;  //!< Publishes controller information
 
     double max_update_time_;
+
+    double last_update_time_;
+
+    double max_allowed_update_time_;
+
+    bool watch_dog_active_;
   };
 
 /** @class WholeBodyTrajectoryControllerNode

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	2009-03-20 23:15:33 UTC (rev 12793)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/whole_body_trajectory_controller.cpp	2009-03-20 23:39:09 UTC (rev 12794)
@@ -90,6 +90,7 @@
       joint_name_.push_back(std::string(jpc->getJointName()));
       current_joint_position_.push_back(0.0);
       current_joint_velocity_.push_back(0.0);
+      joint_errors_.push_back(0.0);
       num_joints++;
     }
     else if(static_cast<std::string>(elt->Attribute("type")) == std::string("BasePIDController"))
@@ -131,7 +132,7 @@
       }
       current_joint_position_.push_back(0.0);
       current_joint_velocity_.push_back(0.0);
-
+      joint_errors_.push_back(0.0);
       num_joints++;
     }
     else if(static_cast<std::string>(elt->Attribute("type")) == std::string("BaseControllerNode"))
@@ -163,6 +164,7 @@
 
   joint_trajectory_->setMaxRates(joint_velocity_limits_);
   joint_trajectory_->setInterpolationMethod(trajectory_type_);
+  joint_trajectory_->setJointWraps(base_joint_index_[2]);
 
   trajectory_point_.setDimension(num_joints);
   dimension_ = num_joints;
@@ -189,6 +191,9 @@
 
   last_time_ = robot_->hw_->current_time_;
 
+  watch_dog_active_ = false;
+  last_update_time_ = robot_->hw_->current_time_;
+
   ROS_INFO("Initialized controllers");
 
   return true;
@@ -237,6 +242,88 @@
   return -1;
 }
 
+bool WholeBodyTrajectoryController::errorsWithinThreshold()
+{
+  for(int i=0; i < dimension_; i++)
+  {
+    if(fabs(joint_errors_[i]) > max_allowable_joint_errors_[i])
+    {
+      return false;
+    }
+  }
+  return true;
+}
+
+void WholeBodyTrajectoryController::computeJointErrors()
+{
+  for(unsigned int i=0; i < joint_pv_controllers_.size(); i++)
+  {
+    if(joint_type_[i] == mechanism::JOINT_CONTINUOUS || joint_type_[i] == mechanism::JOINT_ROTARY)
+    {
+      joint_errors_[i] = angles::shortest_angular_distance(joint_cmd_rt_[i], current_joint_position_[i]);
+    }
+    else //prismatic
+    {
+      joint_errors_[i] = current_joint_position_[i] - joint_cmd_rt_[i];
+    }
+  }
+
+  for(unsigned int i=0; i < base_pid_controller_.size(); i++)
+  {
+    int joint_index = base_joint_index_[i];
+    joint_errors_[joint_index] = joint_cmd_rt_[joint_index] - current_joint_position_[joint_index];
+  }
+}
+
+
+void WholeBodyTrajectoryController::checkWatchDog(double current_time)
+{
+  if(current_time - last_update_time_ < max_allowed_update_time_ && errorsWithinThreshold())
+  {
+    if(watch_dog_active_)
+    {
+      watch_dog_active_ = false;
+    }
+  }
+  else
+  {
+    if(!watch_dog_active_)
+    {
+      watch_dog_active_ = true;
+      stopMotion();
+    }
+  }
+}
+
+void WholeBodyTrajectoryController::stopMotion()
+{
+  std::vector<trajectory::Trajectory::TPoint> tp;
+  int msg_size = 1;
+
+  tp.resize(msg_size+1);
+
+  //set first point in trajectory to current position of the arm
+  tp[0].setDimension((int) dimension_);
+
+  for(int j=0; j < dimension_; j++)
+  {
+    tp[0].q_[j] = current_joint_position_[j];
+    tp[0].time_ = 0.0;
+  }
+  ROS_WARN("Trajectory message has no way points");
+  //set second point in trajectory to current position of the arm
+  tp[1].setDimension((int) dimension_);
+
+  for(int j=0; j < dimension_; j++)
+  {
+    tp[1].q_[j] = current_joint_position_[j];
+    tp[1].time_ = 0.0;
+  }
+  setTrajectoryCmd(tp);
+  base_controller_node_.setCommand(0.0,0.0,0.0);
+
+}
+
 void WholeBodyTrajectoryController::update(void)
 {
 
@@ -248,6 +335,7 @@
 
   current_time_ = robot_->hw_->current_time_;
 
+  checkWatchDog(current_time_);
 
   if(refresh_rt_vals_)
   {
@@ -267,7 +355,7 @@
     {
       joint_cmd_rt_[i] = trajectory_point_.q_[i];
       joint_cmd_dot_rt_[i] = trajectory_point_.qdot_[i];
-//      cout << " " << joint_cmd_rt_[i];
+//      ROS_INFO("Cmd: %f %f",joint_cmd_rt_[i],joint_cmd_dot_rt_[i]);
     }
 //    cout << endl;
     arm_controller_lock_.unlock();
@@ -286,10 +374,10 @@
   for(unsigned int i=0;i<joint_pv_controllers_.size();++i)
     joint_pv_controllers_[i]->setCommand(joint_cmd_rt_[i],joint_cmd_dot_rt_[i]);
 
+  updateBaseController(current_time_);
+
   updateJointControllers();
 
-  updateBaseController(current_time_);
-
 #ifdef PUBLISH_MAX_TIME
   double end_time = realtime_gettime();
   max_update_time_ = std::max(max_update_time_,end_time-start_time);
@@ -328,7 +416,9 @@
     cmd[i] = base_pid_controller_[i].updatePid(error, error_dot, time - last_time_);
     cmd[i] += joint_cmd_dot_rt_[joint_index];
     if(i == 2)
+    {
       theta = current_joint_position_[joint_index];
+    }
   }
 
   //Transform the cmd back into the base frame
@@ -495,6 +585,7 @@
   double scale;
   node_->param<double>(service_prefix_ + "/velocity_scaling_factor",scale,0.25);
   node_->param<double>(service_prefix_ + "/trajectory_wait_timeout",trajectory_wait_timeout_,10.0);
+  node_->param<double>(service_prefix_ + "/trajectory_update_timeout",c_->max_allowed_update_time_,0.2);
 
   c_->velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
 
@@ -551,9 +642,11 @@
 void WholeBodyTrajectoryControllerNode::getJointTrajectoryThresholds()
 {
   c_->goal_reached_threshold_.resize(c_->dimension_);
+  c_->max_allowable_joint_errors_.resize(c_->dimension_);
   for(int i=0; i< c_->dimension_;i++)
   {
     node_->param<double>(service_prefix_ + "/" + c_->joint_name_[i] + "/goal_reached_threshold",c_->goal_reached_threshold_[i],GOAL_REACHED_THRESHOLD);
+    node_->param<double>(service_prefix_ + "/" + c_->joint_name_[i] + "/joint_error_threshold",c_->max_allowable_joint_errors_[i],MAX_ALLOWABLE_JOINT_ERROR_THRESHOLD);
   }
 }
 
@@ -613,6 +706,7 @@
 {
 
   this->ros_lock_.lock();
+  c_->last_update_time_ = c_->current_time_;
   setTrajectoryCmdFromMsg(traj_msg_);
   this->ros_lock_.unlock();
 }

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-03-20 23:15:33 UTC (rev 12793)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_trajectory_controller.cpp	2009-03-20 23:39:09 UTC (rev 12794)
@@ -58,12 +58,18 @@
   robot_msgs::JointTraj cmd;
 
   int num_points = 3;
-  int num_joints = 7;
+  int num_joints = 14;
 
   cmd.set_points_size(num_points);
 
   for(int i=0; i<num_points; i++)
+  {
     cmd.points[i].set_positions_size(num_joints);
+    for(int j=0; j < num_joints; j++)
+    {
+      cmd.points[i].positions[j] = 0.0;
+    }
+  }
 
 /*
   cmd.points[0].positions[0] = 0.0;
@@ -103,23 +109,11 @@
   cmd.points[2].positions[6] = 0.0;
   cmd.points[2].time = 0.0;
 
-  node->advertise<robot_msgs::JointTraj>("arm_trajectory_command",1);
-  node->advertise<robot_msgs::JointTraj>("left_arm_trajectory_controller/arm_trajectory_command",1);
-  node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_controller/arm_trajectory_command",1);
-  sleep(1);
-  node->publish("arm_trajectory_command",cmd);
-  node->publish("left_arm_trajectory_controller/arm_trajectory_command",cmd);
-  node->publish("right_arm_trajectory_controller/arm_trajectory_command",cmd);
+  node->advertise<robot_msgs::JointTraj>("/arm/trajectory_controller/arm_trajectory_command",1);
+  node->publish("/arm/trajectory_controller/arm_trajectory_command",cmd);
   sleep(4);
 
   ros::Time start_time = ros::Time::now();
   ros::Duration sleep_time = ros::Duration().fromSec(0.01);
-
-/*  while(!done)
-  {
-    ros::Duration delta_time = ros::Time::now() - start_time;
-    sleep_time.sleep();
-  }
-*/
   
 }


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12828
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12828&view=rev
Author:   sachinchitta
Date:     2009-03-23 02:10:38 +0000 (Mon, 23 Mar 2009)

Log Message:
-----------
new version of base controller now works

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-03-23 00:57:00 UTC (rev 12827)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-03-23 02:10:38 UTC (rev 12828)
@@ -300,6 +300,10 @@
 
     private:
 
+    double max_acceleration_;
+
+    double max_dirn_change_rate_;
+
     std::vector<double> odometry_residuals_vector_;
 
     bool new_cmd_available_; /** true when new command received by node */

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-03-23 00:57:00 UTC (rev 12827)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-03-23 02:10:38 UTC (rev 12828)
@@ -63,6 +63,9 @@
    max_accel_.y = 0.1;
    max_accel_.z = 0.1;
 
+   max_acceleration_ = 15.0;
+   max_dirn_change_rate_ = 15.0;
+
    cmd_vel_.x = 0;
    cmd_vel_.y = 0;
    cmd_vel_.z = 0;
@@ -82,6 +85,8 @@
 
    cmd_vel_trajectory_ = new trajectory::Trajectory(4);
 
+   cmd_vel_trajectory_->autocalc_timing_ = true;
+
    cmd_vel_points_.resize(2);
    for(int i=0; i< 2; i++)
    {
@@ -90,6 +95,7 @@
       cmd_vel_points_[i].time_ = 0.0;
    }
    cmd_vel_trajectory_->setTrajectory(cmd_vel_points_);
+
    current_vel_point_.setDimension(4);
 
    kp_speed_ = KP_SPEED_DEFAULT;
@@ -305,6 +311,9 @@
   addParamToMap("max_y_accel",&(max_accel_.y));
   addParamToMap("max_yaw_accel",&(max_accel_.z));
 
+  addParamToMap("max_accel",&(max_acceleration_));
+  addParamToMap("max_dirn_change_rate",&(max_dirn_change_rate_));
+
   elt = config->FirstChildElement("map");
 
   while(elt)
@@ -323,6 +332,17 @@
   }
 
 
+   std::vector<double> velocity_limits;
+   velocity_limits.push_back(max_dirn_change_rate_);
+   velocity_limits.push_back(max_dirn_change_rate_);
+   velocity_limits.push_back(max_dirn_change_rate_);
+   velocity_limits.push_back(max_acceleration_);
+
+  cmd_vel_trajectory_->setMaxRates(velocity_limits);
+  cmd_vel_trajectory_->setInterpolationMethod("linear");
+
+
+
   ROS_INFO("BaseControllerPos:: kp_speed %f",kp_speed_);
   ROS_INFO("BaseControllerPos:: kp_caster_steer  %f",caster_steer_vel_gain_);
   ROS_INFO("BaseControllerPos:: timeout %f",timeout_);
@@ -332,6 +352,8 @@
   ROS_INFO("BaseControllerPos:: max_x_accel %f",(max_accel_.x));
   ROS_INFO("BaseControllerPos:: max_y_accel %f",(max_accel_.y));
   ROS_INFO("BaseControllerPos:: max_yaw_accel %f",(max_accel_.z));
+  ROS_INFO("BaseControllerPos:: max_dirn_change_rate %f",(max_dirn_change_rate_));
+  ROS_INFO("BaseControllerPos:: max_accel %f",(max_acceleration_));
 
   init(jcp_vec,robot_state);
 
@@ -480,7 +502,7 @@
    double new_cmd_mag = sqrt(new_cmd.x*new_cmd.x+new_cmd.y*new_cmd.y+new_cmd.z*new_cmd.z);
    libTF::Vector new_vel_direction;
    double dot_direction(0), delta_direction(0), delta_direction_m_pi(0);
-
+   double epsilon_direction_difference(0.2);
    if(max_rate.x > EPS)
       dt_min_x = fabs(new_cmd.x-cmd_vel_.x)/max_rate.x;
    if(max_rate.y > EPS)
@@ -499,6 +521,8 @@
    cmd_vel_points_[0].q_[3] = cmd_vel_magnitude_;
    cmd_vel_points_[0].time_ = 0.0;
 
+
+
    if(new_cmd_mag > EPS)
    {
       new_vel_direction.x = new_cmd.x/new_cmd_mag;
@@ -515,6 +539,13 @@
        new_vel_direction.z = -new_vel_direction.z;
        new_cmd_mag = new_cmd.x*new_vel_direction.x + new_cmd.y*new_vel_direction.y + new_cmd.z*new_vel_direction.z;
      } 
+     if(fabs(cmd_vel_magnitude_) < EPS)
+     {
+       if(fabs(new_vel_direction.x - cmd_vel_direction_.x) > epsilon_direction_difference || fabs(new_vel_direction.y - cmd_vel_direction_.y) >  epsilon_direction_difference  || fabs(new_vel_direction.z - cmd_vel_direction_.z) >  epsilon_direction_difference)
+       {
+         new_cmd_mag = 0.0;
+       }
+     }
    }
    else
    {
@@ -528,7 +559,8 @@
    cmd_vel_points_[1].q_[1] = new_vel_direction.y;
    cmd_vel_points_[1].q_[2] = new_vel_direction.z;
    cmd_vel_points_[1].q_[3] = new_cmd_mag;
-   cmd_vel_points_[1].time_ = dt;
+//   cmd_vel_points_[1].time_ = dt;
+   cmd_vel_points_[1].time_ = 0.0;
 
    cmd_vel_trajectory_->setTrajectory(cmd_vel_points_);
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 12925
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12925&view=rev
Author:   meeussen
Date:     2009-03-24 21:20:05 +0000 (Tue, 24 Mar 2009)

Log Message:
-----------
add service to set periodic command with angles and duration

Modified Paths:
--------------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
    pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2009-03-24 21:07:11 UTC (rev 12924)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2009-03-24 21:20:05 UTC (rev 12925)
@@ -53,6 +53,7 @@
 #include <robot_mechanism_controllers/SetCommand.h>
 #include <robot_mechanism_controllers/GetCommand.h>
 #include <pr2_mechanism_controllers/SetProfile.h>
+#include <pr2_mechanism_controllers/SetPeriodicCmd.h>
 
 #include "boost/thread/mutex.hpp"
 #include "trajectory/trajectory.h"
@@ -70,11 +71,11 @@
 
   virtual void update() ;
 
-  void setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
+  bool setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
 
-  void setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
+  bool setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
 
-  void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
+  bool setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
                      double max_rate, double max_acc, std::string interp ) ;
 
   //! \brief Returns what time we're currently at in the profile being executed
@@ -131,6 +132,8 @@
   // Message Callbacks
   void setPeriodicCmd() ;
   void setTrackLinkCmd() ;
+  bool setPeriodicSrv(pr2_mechanism_controllers::SetPeriodicCmd::Request &req, 
+                      pr2_mechanism_controllers::SetPeriodicCmd::Response &res);
 
 private:
   ros::Node *node_ ;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg	2009-03-24 21:07:11 UTC (rev 12924)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg	2009-03-24 21:20:05 UTC (rev 12925)
@@ -4,4 +4,4 @@
 
 # signal == 0 => Half profile complete
 # signal == 1 => Full Profile Complete
-uint8 signal
+int32 signal

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-03-24 21:07:11 UTC (rev 12924)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2009-03-24 21:20:05 UTC (rev 12925)
@@ -227,7 +227,7 @@
   return traj_duration_ ;
 }
 
-void LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
+bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
 {
   while (!traj_lock_.try_lock())
     usleep(100) ;
@@ -248,9 +248,11 @@
   traj_duration_ = traj_.getTotalTime() ;
 
   traj_lock_.unlock() ;
+
+  return true;
 }
 
-void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd)
+bool LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd)
 {
   if (cmd.profile == "linear" ||
       cmd.profile == "blended_linear")
@@ -276,16 +278,23 @@
     cur_point.time_ = cmd.period ;
     tpoints.push_back(cur_point) ;
 
-    setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile) ;
-    ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
+    if (!setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile)){
+      ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
+      return false;
+    }
+    else{
+      ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
+      return true;
+    }
   }
   else
   {
     ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
+    return false;
   }
 }
 
-void LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
+bool LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
 {
   while (!track_link_lock_.try_lock())
     usleep(100) ;
@@ -317,6 +326,8 @@
   }
 
   track_link_lock_.unlock() ;
+
+  return track_link_enabled_;
 }
 
 
@@ -332,6 +343,8 @@
   node_->unsubscribe(service_prefix_ + "/set_periodic_cmd") ;
   node_->unsubscribe(service_prefix_ + "/set_track_link_cmd") ;
 
+  node_->unadvertiseService(service_prefix_ + "/set_periodic_cmd");
+
   publisher_->stop() ;
 
   delete publisher_ ;    // Probably should wait on publish_->is_running() before exiting. Need to
@@ -396,6 +409,8 @@
   node_->subscribe(service_prefix_ + "/set_periodic_cmd", cmd_, &LaserScannerTrajControllerNode::setPeriodicCmd, this, 1) ;
   node_->subscribe(service_prefix_ + "/set_track_link_cmd", track_link_cmd_, &LaserScannerTrajControllerNode::setTrackLinkCmd, this, 1) ;
 
+  node_->advertiseService(service_prefix_ + "/set_periodic_cmd", &LaserScannerTrajControllerNode::setPeriodicSrv, this);
+
   if (publisher_ != NULL)               // Make sure that we don't memory leak if initXml gets called twice
     delete publisher_ ;
   publisher_ = new realtime_tools::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
@@ -407,6 +422,19 @@
   return true ;
 }
 
+bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_mechanism_controllers::SetPeriodicCmd::Request &req, 
+                                                    pr2_mechanism_controllers::SetPeriodicCmd::Response &res)
+{
+  cout << "laser scanner controller: set periodic command" << endl;
+
+  if (!c_.setPeriodicCmd(req.command))
+    return false;
+  else{
+    res.start_time = ros::Time::now();
+    return true;
+  }
+}
+
 void LaserScannerTrajControllerNode::setPeriodicCmd()
 {
   c_.setPeriodicCmd(cmd_) ;
@@ -416,3 +444,4 @@
 {
   c_.setTrackLinkCmd(track_link_cmd_) ;
 }
+

Added: pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/srv/SetPeriodicCmd.srv	2009-03-24 21:20:05 UTC (rev 12925)
@@ -0,0 +1,3 @@
+PeriodicCmd command
+---
+time start_time
\ No newline at end of file


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.

Revision: 13934
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13934&view=rev
Author:   sachinchitta
Date:     2009-04-16 02:57:07 +0000 (Thu, 16 Apr 2009)

Log Message:
-----------
cleaned up version of arm_trajectory controller, will eventually replace arm_trajectory_controller after extensive testing

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp

Added: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/joint_trajectory_controller.h	2009-04-16 02:57:07 UTC (rev 13934)
@@ -0,0 +1,484 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#pragma once
+
+#include <ros/node.h>
+#include <boost/thread/mutex.hpp>
+
+#include <mechanism_model/controller.h>
+#include <robot_mechanism_controllers/joint_pd_controller.h>
+
+// Services
+#include <robot_msgs/JointTraj.h>
+#include <robot_msgs/JointTrajPoint.h>
+#include <robot_msgs/DiagnosticMessage.h>
+
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <pr2_mechanism_controllers/base_controller.h>
+
+//Kinematics
+#include <trajectory/trajectory.h>
+#include <robot_msgs/ControllerState.h>
+
+#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_tools.h>
+#include <std_msgs/String.h>
+
+#include <angles/angles.h>
+
+namespace controller
+{
+
+    const std::string JointTrajectoryStatusString[7] = {"0 - ACTIVE","1 - DONE","2 - QUEUED","3 - DELETED","4 - FAILED","5 - CANCELED","6 - NUM_STATUS"};
+
+#define GOAL_REACHED_THRESHOLD 0.01
+#define MAX_ALLOWABLE_JOINT_ERROR_THRESHOLD 0.2
+  // comment this out if the controller is not supposed to publish its own max execution time
+#define PUBLISH_MAX_TIME
+
+
+/** @class JointTrajectoryController
+ *  @brief ROS interface for a joint trajectory controller.
+ *  @author Sachin Chitta <sachinc@...>
+ *
+ *  This class provides a ROS interface for controlling a set of joints by setting position configurations. If offers several ways to control the joints:
+ *  - through listening to ROS messages: this is specified in the XML configuration file by the following parameters:
+ *      <listen_topic name="the name of my message" />
+ *      (only one topic can be specified)
+ *  - through a non blocking service call: this service call can specify a single trajectory as a goal
+ *
+ */
+  class JointTrajectoryController : public Controller
+  {
+    public:
+
+    /**
+     * @brief Default Constructor of the JointController class.
+     *
+     */
+    JointTrajectoryController();
+
+    /*!
+     * \brief Destructor of the JointController class. Stops publishers and services.
+     */
+    virtual ~JointTrajectoryController();
+
+    /*!
+     * @brief Initialize the controller using an Xml configuration and a robot object.
+     * @param robot pointer to a robot object passed in by all controllers.
+     * @config TiXml configuration element
+     */
+    bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
+
+    /*!
+     * @brief Issues commands to the joints and is called at regular intervals in realtime. This function is required 
+     * to be realtime safe.
+     */
+    virtual void update(void); 
+
+    private:
+
+    boost::mutex arm_controller_lock_; /**< Mutex lock for sharing information between services and realtime */
+
+    double last_time_; /**< last time update was called */
+
+    std::vector<control_toolbox::Pid> base_pid_controller_;      /**< Internal PID controllers for controlling the base. */
+
+    std::vector<int> base_joint_index_; /**< index into the list of joints for the virtual joints corresponding to the base */
+
+    std::vector<std::string> joint_name_; /**< names of joints controlled by this controller */
+
+    std::vector<int> joint_type_; /**< joint types of the joints controlled by this controller, these are derived from the mechanism class */
+
+    std::vector<double> current_joint_position_; /**< internal storage for the current joint positions */
+
+    std::vector<double> current_joint_velocity_;  /**< internal storage for the current joint velocities */
+
+    std::vector<double> joint_position_errors_;  /**< internal storage for the joint position errors (actual - command)*/
+
+    std::vector<double> joint_velocity_errors_;  /**< internal storage for the joint velocity errors (actual - command)*/
+
+    std::vector<double> max_allowable_joint_errors_;  /**< max allowable joint errors. These are loaded through the ROS Param server. e.g. <param name="r_shoulder_pan_joint/joint_error_threshold" type="double" value="0.1"/>*/
+
+    controller::BaseControllerNode base_controller_node_; /**< internal controller used to start up a controller for a robot base */
+
+    std::vector<JointPDController *> joint_pv_controllers_; /**< internal set of controllers used to control individual joints */
+
+    trajectory::Trajectory *joint_trajectory_; /**< internal representation of the trajectory specified for all the joints */
+
+    std::vector<double> joint_cmd_; /**< internal representation of the instantaneous position commands for each joint */
+
+    std::vector<double> joint_cmd_dot_; /**< internal representation of the instantaneous velocity commands for each joint */
+
+    mechanism::Robot* robot_; /**< Pointer to a robot object */
+
+    mechanism::RobotState* robot_state_; /**< Pointer to a robot state object */
+
+    bool refresh_rt_vals_; /**< Indicates that a new trajectory has been received. */
+
+    double trajectory_start_time_; /**< Start time for the current trajectory */
+
+    double trajectory_end_time_;  /**< End time for the current trajectory */
+
+    double current_time_; /**< Current time */
+
+    bool trajectory_done_; /**< Indicates that the current trajectory is done */
+
+    int num_joints_; /**< Number of joints controlled by this controller */
+
+    std::vector<double> joint_velocity_limits_; /**< Velocity limits for each joint. These are derived from the robot object created using a representation of the robot */
+
+    std::string trajectory_type_; /**< The trajectory type used for control. It can be "linear", "cubic"*/
+
+    double velocity_scaling_factor_; /**< Scaling factor for the velocity limits */
+
+    double trajectory_wait_time_; /**< The amount of time that the controller waits after the expected completion time for a trajectory before declaring that a trajectory has failed */
+
+    std::vector<double> goal_reached_threshold_; /**< Threshold within which the joints must be before goal is declared to have been reached */
+
+    realtime_tools::RealtimePublisher <robot_msgs::ControllerState>* controller_state_publisher_ ;  /**< Publishes controller information */
+
+    double max_update_time_; /**< maximum time (over the complete history of the run) taken for the update loop of this controller*/
+
+    double last_traj_req_time_; /**< Last time a trajectory command was received on a topic or service */
+
+    double max_allowed_update_time_; /**< Safety behavior: Max allowed time between commands */
+
+    bool watch_dog_active_; 
+
+    realtime_tools::RealtimePublisher <robot_msgs::DiagnosticMessage>* diagnostics_publisher_ ;  /**< Publishes controller information as a diagnostic message */
+
+    /*!
+     * \brief mutex lock for setting and getting ros messages
+     */
+    boost::mutex ros_lock_;
+
+    robot_msgs::JointTraj traj_msg_; /**< The trajectory message received over ROS */
+
+    /*!
+     * \brief node name
+     */
+    std::string name_;
+
+    /*
+     * \brief Topic on which the controller listens for commands
+     */
+    std::string listen_topic_name_;
+
+    /*
+     * \brief pointer to ros node
+     */
+    ros::Node * const node_;
+
+    std::vector<robot_msgs::JointTraj> joint_trajectory_vector_; /**< Vector of trajectory requests */
+
+    std::vector<int> joint_trajectory_id_; /**< Vector of ids for trajectory requests */
+
+    /**
+     * @brief Get the parameters from the ROS parameter server 
+     */
+    void getParams(); 
+
+    /**
+     * @brief Initialize publishers (diagnostic, state) 
+     */
+    void initializePublishers();
+
+    /**
+     * @brief Stop publishers (diagnostic, state) 
+     */
+    void stopPublishers();
+
+    /**
+     * @brief Advertise all services 
+     */
+    void advertiseServices();
+
+    /**
+     * @brief Unadvertise all services 
+     */
+    void unadvertiseServices();
+
+    /**
+     * @brief Subscribe to topics
+     */
+    void subscribeTopics();
+
+    /**
+     * @brief Unsubscribe to topics
+     */
+    void unsubscribeTopics();
+
+    /**
+     * @brief Load parameters from an XML file 
+     * @param robot pointer to a robot object passed in by all controllers.
+     * @config TiXml configuration element
+     */
+    bool loadXmlFile(mechanism::RobotState * robot, TiXmlElement * config);
+
+    /**
+     * @brief Initialize the trajectory from a tiny XML element 
+     */
+    void initTrajectory(TiXmlElement * config);
+
+    /**
+     * @brief Set the trajectory command to the current values of each joint. This has the effect of stopping all joints
+     * at their current positions
+     */
+    void setTrajectoryCmdToCurrentValues();
+
+    /**
+     * @brief Add a joint to the list of joints controlled by this controller.
+     * @param name The name of the joint in the mechanism structure.
+     */
+    void addJoint(const std::string &name);
+
+    /**
+     * @brief Add a controller to control the base.
+     * @param elt A tinyXML element required to initialize the base controller
+     */
+    void addBaseController(TiXmlElement *elt);
+
+    /**
+     * @brief Set the commanded trajectory for the controller
+     * @param joint_trajectory A vector of trajectory points that this controller must follow 
+     */
+    bool setTrajectoryCmd(const std::vector<trajectory::Trajectory::TPoint>& joint_trajectory);
+
+    /**
+     * @brief Determine if the joint position errors are within the threshold specified by goal_reached_threshold. 
+     * These thresholds are set using the ROS param server e.g. <param name="r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/> 
+     * @return true if absolute position errors are less than threshold values, false otherwise
+     */
+    bool errorsWithinThreshold();
+
+    /**
+     * @brief Determine if the joints are at rest. 
+     * @return true if absolute velocities are less than zero
+     */
+    bool atRest();
+
+    /**
+     * @brief Compute the errors for each joint
+     */
+    void computeJointErrors();
+
+    /**
+     * @brief Check watchdog. If there has been no trajectory request externally for a while or if the position errors are too great, this will set the current position to the desired position, thus effectively stopping the robot.
+     * The parameters that affect this function are 
+     * (a) max_allowed_update_time_ which can be set using a ROS param call, e.g. <param name="max_allowed_update_time" type="double" value="0.1"/>
+     * (b) max_allowable_joint_errors_ which can be set individually for each joint using a ROS param call, e.g. <param name="r_shoulder_pan_joint/joint_error_threshold" type="double" value="0.1"/>
+     */
+    void checkWatchDog(double current_time);
+
+    /**
+     * @brief Stop the motion of all joints. In addition to setting all desired joint positions to the current position, it also sets velocities for the base to zero 
+     */
+    void stopMotion();
+
+    /**
+     * @brief Reset all the trajectory times on completion of an old trajectory or receipt of a new one that preempts the previous one 
+     */
+    void resetTrajectoryTimes();
+
+    /**
+     * @brief Internal function to get the actual set point values from the trajectory.
+     */
+    void getJointCommands();
+
+    /**
+     * @brief Check if the trajectory is done. If done, pop the next trajectory from the queue and use it. If no new trajectory is available, set the desired joint positions to the current joint positions
+     */
+    void checkIfTrajectoryDone();
+
+    /** 
+     * @brief Based on the current position of the base, update the velocity commands to be sent out to the base controller
+     */
+    void updateBaseCommand(double time);
+
+    /**   
+     * @brief Check if the goal position has been reached 
+     */
+    bool reachedGoalPosition(std::vector<double> joint_cmd);
+
+    /**   
+     * @brief Call update() on each joint controller. (This actually sends out the commands into hardware)
+     */
+    void updateJointControllers(void);
+
+    /**   
+     * @brief Update all joint values by reading from the robot structure
+     */
+    void updateJointValues();
+
+    /**   
+     * @brief Update the trajectory queue by removing trajectories that are done, set the current trajectory to be executed to the next trajectory on the queue.
+     * If no new trajectory is available, set the desired joint positions to the current joint positions
+     * @param finish_status The status of the last trajectory
+     */
+    void updateTrajectoryQueue(int finish_status);
+
+    /**   
+     * @brief Convert a trajectory command (from the queue) and set the current desired trajectory to it
+     * @param traj_msg Trajectory message received on a topic or a service
+     * @param id The associated id for the trajectory command
+     */
+    void setTrajectoryCmdFromMsg(robot_msgs::JointTraj traj_msg, int id);
+
+    /**   
+     * @brief Callback when a trajectory message is received on a topic
+     */
+    void TrajectoryReceivedOnTopic();
+
+    /**   
+     * @brief Service provided to set trajectories
+     * @param req The request containing the trajectory to be queued up
+     * @param resp The response contains the id assigned to the trajectory
+     */
+    bool setJointTrajSrv(pr2_mechanism_controllers::TrajectoryStart::Request &req,
+                                                pr2_mechanism_controllers::TrajectoryStart::Response &resp);
+
+    /**   
+     * @brief Service provided to set trajectories
+     * @param req The request contains the id of the trajectory about which you need information (Use the rosmsg tool to see the fields required for the request. e.g. rosmsg show TrajectoryQuery)
+     * @param resp The response contains information about the trajectory in the following fields: 
+     *             (a) done: 1 if trajectory is done, 0 if ongoing, 2 if queued, 3 if deleted, 4 if failed, 5 if canceled 
+     *             (b) trajectorytime: If active, the current timestamp the trajectory is at, if done the total time taken for the trajectory, if queued 0
+     *             (c) jointnames: the names of the joints controlled by this controller
+     *             (d) jointpositions: the current joint positions 
+     */
+    bool queryJointTrajSrv(pr2_mechanism_controllers::TrajectoryQuery::Request &req,
+                                                  pr2_mechanism_controllers::TrajectoryQuery::Response &resp);
+
+    /**   
+     * @brief Service provided to cancel trajectories
+     * @param req The request contains the id of the trajectory which needs to be canceled (Use the rosmsg tool to see the fields required for the request. e.g. rosmsg show TrajectoryCancel)
+     */
+    bool cancelJointTrajSrv(pr2_mechanism_controllers::TrajectoryCancel::Request &req,
+                                                   pr2_mechanism_controllers::TrajectoryCancel::Response &resp);
+
+
+    /**   
+     * @brief Create a vector of trajectory point(TPoint) objects from a trajectory message
+     * @param new_traj The trajectory message that needs to be converted
+     * @param tp The resultant vector of TPoints
+     */
+    bool createTrajectoryPointsVectorFromMsg(const robot_msgs::JointTraj &new_traj, std::vector<trajectory::Trajectory::TPoint> &tp);
+
+    /**   
+     * @brief Create a trajectory object from a trajectory message
+     * @param new_traj The trajectory message that needs to be converted
+     * @param return_trajectory The resultant trajectory object
+     */
+    bool createTrajectoryFromMsg(const robot_msgs::JointTraj &new_traj,trajectory::Trajectory &return_trajectory);
+
+    /**   
+     * @brief Add a new trajectory request to the queue of trajectories that need to be sent out
+     * @param new_traj The trajectory message that needs to be queued
+     * @param id The id of the trajectory to be queued
+     */
+    void addTrajectoryToQueue(robot_msgs::JointTraj new_traj, int id);
+
+    /**   
+     * @brief Preempt the current trajectory queue
+     * @param new_traj The trajectory message that needs to executed
+     * @param id The id of the trajectory
+     */
+    void preemptTrajectoryQueue(robot_msgs::JointTraj new_traj, int id);
+
+    /**   
+     * @brief Delete a trajectory from the queue of trajectories
+     * @param new_traj The trajectory message that needs to be deleted
+     */
+    void deleteTrajectoryFromQueue(int id);
+
+    /**   
+     * @brief Publish diagnostic information
+     */
+    void publishDiagnostics();
+
+    /**   
+     * @brief Initialized to 1 (0 represents a trajectory that keeps the robot at its current position)
+     */
+    int request_trajectory_id_;
+
+    /**   
+     * @brief Initialized to 0 and then set to the requested trajectory id. Reverts to 0 when no trajectories are available on the queue
+     */
+    int current_trajectory_id_;
+
+    /**   
+     * @brief A map from trajectory ids to trajectory status
+     */
+    std::map<int,int> joint_trajectory_status_;
+
+    /**   
+     * @brief A map from trajectory ids to trajectory times
+     */
+    std::map<int,double>joint_trajectory_time_;
+
+    /**   
+     * @brief Enumeration of trajectory status
+     */
+    enum JointTrajectoryStatus{
+      ACTIVE,/*!< The current trajectory being executed. */
+      DONE,/*!< Trajectory execution was finished. */
+      QUEUED, /*!< Trajectory is queued for execution. */
+      DELETED, /*!< Trajectory has been deleted BEFORE execution. */
+      FAILED, /*!< Trajectory execution has failed. This happens if the joints fail to get to the last desired position in the trajectory. */
+      CANCELED, /*!< Trajectory has been canceled (preempted by other trajectory) while active */
+      NUM_STATUS
+    };
+
+    /**   
+     * @brief The maximum amount of time that the controller waits after the expected completion time for a trajectory before declaring that a trajectory has failed 
+     */
+    double trajectory_wait_timeout_;
+
+    double last_diagnostics_publish_time_; /**< Last time diagnostics infomation was published */
+
+    double diagnostics_publish_delta_time_;  /**< Expected rate at which diagnostics information should be published. This can be published using a ROS param call  <param name="diagnostics_publish_delta_time" type="double" value="0.5"/> */
+
+    double at_rest_velocity_threshold_; /**< Threshold for deciding if all the joints are at rest */
+
+    std::vector<trajectory::Trajectory::TPoint> current_joint_position_vector_; /**< Pre-allocated to size 2 x num_joints in the constructor to have a realtime safe container for the current position of the joints */
+
+    trajectory::Trajectory::TPoint trajectory_point_; /**< Pre-allocated to size num_joints in the constructor to have a realtime safe container for the current position of the joints */
+
+    int base_theta_index_; /**< Index corresponding to the rotational degree of freedom of a robot base */
+  };
+}

Added: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp	2009-04-16 02:57:07 UTC (rev 13934)
@@ -0,0 +1,960 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// Original version: Sachin Chitta <sachinc@...>
+
+#include "pr2_mechanism_controllers/joint_trajectory_controller.h"
+
+using namespace controller;
+using namespace std;
+
+ROS_REGISTER_CONTROLLER(JointTrajectoryController);
+
+JointTrajectoryController::JointTrajectoryController() : Controller(), node_(ros::Node::instance())
+{
+  controller_state_publisher_ = NULL;
+  num_joints_ = 0;
+  refresh_rt_vals_= false;
+  trajectory_type_ = "linear";
+  trajectory_wait_time_= 0.0;
+  max_update_time_= 0.0;
+  watch_dog_active_ = false;
+  request_trajectory_id_ = 1; 
+  current_trajectory_id_ = 0; 
+  trajectory_wait_timeout_ = 10.0;
+  diagnostics_publisher_ = NULL;
+}
+
+
+JointTrajectoryController::~JointTrajectoryController()
+{
+  for(unsigned int i=0; i<joint_pv_controllers_.size();++i)
+    delete joint_pv_controllers_[i];
+
+  unadvertiseServices();
+  unsubscribeTopics();
+
+  delete joint_trajectory_;
+}
+
+bool JointTrajectoryController::initXml(mechanism::RobotState * robot, TiXmlElement * config)
+{
+  name_ = config->Attribute("name");
+
+  loadXmlFile(robot,config);
+
+  getParams();
+
+  advertiseServices();
+
+  initializePublishers();
+
+  ROS_INFO("Loaded JointTrajectoryController: %s",name_.c_str());
+  return true;
+}
+
+
+void JointTrajectoryController::getParams()
+{
+  double scale;
+  node_->param<double>("~velocity_scaling_factor",scale,0.25);
+  node_->param<double>("~trajectory_wait_timeout",trajectory_wait_timeout_,10.0);
+  node_->param<double>("~trajectory_update_timeout",max_allowed_update_time_,0.2);
+  node_->param<std::string>("~listen_topic_name",listen_topic_name_,"command");
+  listen_topic_name_ = "~" + listen_topic_name_;
+  node_->param<double>("~at_rest_velocity_threshold_",at_rest_velocity_threshold_,1e-5);
+  node_->param<double>("~max_allowed_update_time_",max_allowed_update_time_,0.1);
+  node_->param<double>("~diagnostics_publish_delta_time",diagnostics_publish_delta_time_,1.0);
+  velocity_scaling_factor_ = std::min(1.0,std::max(0.0,scale));
+
+  goal_reached_threshold_.resize(num_joints_);
+  max_allowable_joint_errors_.resize(num_joints_);
+  for(int i=0; i< num_joints_;i++)
+  {
+    node_->param<double>("~" + joint_name_[i] + "/goal_reached_threshold",goal_reached_threshold_[i],GOAL_REACHED_THRESHOLD);
+    node_->param<double>("~" + joint_name_[i] + "/joint_error_threshold",max_allowable_joint_errors_[i],MAX_ALLOWABLE_JOINT_ERROR_THRESHOLD);
+  }
+}
+
+void JointTrajec