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

[<vij...@us...>]

Revision: 8243
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8243&view=rev
Author:   vijaypradeep
Date:     2008-12-17 02:02:47 +0000 (Wed, 17 Dec 2008)

Log Message:
-----------
TrackLinkCmd works in gazebo. Need to test it on the robot. Need to also add a filter to this

Modified 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
    pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml

Added Paths:
-----------
    pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
    pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py

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	2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h	2008-12-17 02:02:47 UTC (rev 8243)
@@ -44,6 +44,7 @@
 // Messages
 #include <pr2_mechanism_controllers/LaserScannerSignal.h>
 #include <pr2_mechanism_controllers/PeriodicCmd.h>
+#include <pr2_mechanism_controllers/TrackLinkCmd.h>
 
 // Services
 #include <robot_mechanism_controllers/SetCommand.h>
@@ -68,6 +69,8 @@
 
   void setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
 
+  void setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
+
   void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
                      double max_rate, double max_acc, std::string interp ) ;
 
@@ -83,11 +86,19 @@
   ros::thread::mutex traj_lock_ ;                                       // Mutex for traj_
   trajectory::Trajectory traj_ ;                                        // Stores the current trajectory being executed
 
+  ros::thread::mutex track_link_lock_ ;
+  bool track_link_enabled_ ;
+  mechanism::LinkState* target_link_ ;
+  mechanism::LinkState* mount_link_ ;
+  tf::Vector3 track_point_ ;
+
   JointPositionSmoothController joint_position_controller_ ;            // The PID position controller that is doing all the under-the-hood controls stuff
 
   double traj_start_time_ ;                                             // The time that the trajectory was started (in seconds)
   double traj_duration_ ;                                               // The length of the current profile (in seconds)
 
+  double tracking_offset_ ;                                             // Offset generated by the track_link code
+
   double max_rate_ ;                                                    // Max allowable rate/velocity
   double max_acc_ ;                                                     // Max allowable acceleration
 };
@@ -104,6 +115,7 @@
 
   // Message Callbacks
   void setPeriodicCmd() ;
+  void setTrackLinkCmd() ;
 
 private:
   ros::node *node_ ;
@@ -113,6 +125,7 @@
   double prev_profile_time_ ;                                                    //!< The time in the current profile when update() was last called
 
   pr2_mechanism_controllers::PeriodicCmd cmd_ ;
+  pr2_mechanism_controllers::TrackLinkCmd track_link_cmd_ ;
 
   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.

Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg	2008-12-17 02:02:47 UTC (rev 8243)
@@ -0,0 +1,3 @@
+int8 enable
+string link_name
+std_msgs/Point point

Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py	                        (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py	2008-12-17 02:02:47 UTC (rev 8243)
@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG) 
+
+import sys
+import os
+import string
+
+import rospy
+from std_msgs import *
+
+from std_msgs.msg import Point
+from pr2_mechanism_controllers.msg import TrackLinkCmd
+from time import sleep
+
+def print_usage(exit_code = 0):
+    print '''Usage:
+    send_track_link_cmd.py [controller] [link_name] [x] [y] [z]
+       - [controller] - The controller's name
+       - [link name]  - Name of the link that we want to track
+       - [x] [y] [z]  - The location on the link (in the link's frame) that we want to track
+    send_track_link_cmd.py [controller]
+       - Sends a 'disable' command to the controller
+'''
+    sys.exit(exit_code)
+
+if __name__ == '__main__':
+    if len(sys.argv) == 6 :
+        cmd = TrackLinkCmd()
+        controller =    sys.argv[1]
+        cmd.link_name = sys.argv[2] 
+        cmd.point     = Point(float(sys.argv[3]), float(sys.argv[4]), float(sys.argv[5]))
+        cmd.enable    = 1
+    elif len(sys.argv) == 2 :
+        cmd = TrackLinkCmd()
+        controller =    sys.argv[1]
+        cmd.link_name = 'none'
+        cmd.point     = Point(0.0, 0.0, 0.0)
+        cmd.enable    = 0
+    else :
+        print_usage()
+
+    print 'Sending TrackLinkCmd Command to %s: ' % controller
+    print '  Enable:    %u' % cmd.enable
+    print '  Link Name: %s' % cmd.link_name
+    print '  Point:     (%f, %f, %f)' % (cmd.point.x, cmd.point.y, cmd.point.z)
+
+    command_publisher = rospy.Publisher(controller + '/set_track_link_cmd', TrackLinkCmd)
+    rospy.init_node('track_link_cmd_commander', anonymous=True)
+    sleep(1)
+    command_publisher.publish( cmd )
+
+    sleep(1)
+    print 'Command sent!'
+


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

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	2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp	2008-12-17 02:02:47 UTC (rev 8243)
@@ -44,7 +44,8 @@
 
 LaserScannerTrajController::LaserScannerTrajController() : traj_(1)
 {
-
+  tracking_offset_ = 0 ;
+  track_link_enabled_ = false ;
 }
 
 LaserScannerTrajController::~LaserScannerTrajController()
@@ -92,6 +93,32 @@
 {
   //if (!joint_state_->calibrated_)
   //  return;
+
+
+  // ***** Compute the offset from tracking a link *****
+  //! \todo replace this link tracker with a KDL inverse kinematics solver
+  if(track_link_lock_.trylock())
+  {
+    if (track_link_enabled_  && target_link_ && mount_link_)
+    {
+      // Compute the position of track_point_ in the world frame
+      tf::Pose link_pose(target_link_->abs_orientation_, target_link_->abs_position_) ;
+      tf::Point link_point_world ;
+      link_point_world = link_pose*track_point_ ;
+
+      // We're hugely approximating our inverse kinematics. This is probably good enough for now...
+      double dx = link_point_world.x() - mount_link_->abs_position_.x() ;
+      double dz = link_point_world.z() - mount_link_->abs_position_.z() ;
+      tracking_offset_ = atan2(-dz,dx) ;
+    }
+    else
+    {
+      tracking_offset_ = 0.0 ;
+    }
+    track_link_lock_.unlock() ;
+  }
+
+  // ***** Compute the current command from the trajectory profile *****
   if (traj_lock_.trylock())
   {
     if (traj_duration_ > 1e-6)                                   // Short trajectories could make the mod_time calculation unstable
@@ -107,7 +134,7 @@
       result = traj_.sample(sampled_point, profile_time) ;
       if (result > 0)
       {
-        joint_position_controller_.setCommand(sampled_point.q_[0]) ;
+        joint_position_controller_.setCommand(sampled_point.q_[0] + tracking_offset_) ;
         joint_position_controller_.update() ;
       }
     }
@@ -151,13 +178,13 @@
   traj_lock_.unlock() ;
 }
 
-void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd_)
+void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd)
 {
-  if (cmd_.profile == "linear" ||
-      cmd_.profile == "blended_linear")
+  if (cmd.profile == "linear" ||
+      cmd.profile == "blended_linear")
   {
-    double high_pt = cmd_.amplitude + cmd_.offset ;
-    double low_pt = -cmd_.amplitude + cmd_.offset ;
+    double high_pt = cmd.amplitude + cmd.offset ;
+    double low_pt = -cmd.amplitude + cmd.offset ;
 
     std::vector<trajectory::Trajectory::TPoint> tpoints ;
 
@@ -170,14 +197,14 @@
     tpoints.push_back(cur_point) ;
 
     cur_point.q_[0] = high_pt ;
-    cur_point.time_ = cmd_.period/2.0 ;
+    cur_point.time_ = cmd.period/2.0 ;
     tpoints.push_back(cur_point) ;
 
     cur_point.q_[0] = low_pt ;
-    cur_point.time_ = cmd_.period ;
+    cur_point.time_ = cmd.period ;
     tpoints.push_back(cur_point) ;
 
-    setTrajectory(tpoints, max_rate_, max_acc_, cmd_.profile) ;
+    setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile) ;
     ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
   }
   else
@@ -186,6 +213,41 @@
   }
 }
 
+void LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
+{
+  while (!track_link_lock_.trylock())
+    usleep(100) ;
+
+  if (track_link_cmd.enable)
+  {
+    ROS_INFO("LaserScannerTrajController:: Tracking link %s", track_link_cmd.link_name.c_str()) ;
+    track_link_enabled_ = true ;
+    string mount_link_name = "laser_tilt_mount_link" ;
+    target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
+    mount_link_  = robot_->getLinkState(mount_link_name) ;
+    tf::PointMsgToTF(track_link_cmd.point, track_point_) ;
+
+    if (target_link_ == NULL)
+    {
+      ROS_ERROR("LaserScannerTrajController:: Could not find target link:%s", track_link_cmd.link_name.c_str()) ;
+      track_link_enabled_ = false ;
+    }
+    if (mount_link_ == NULL)
+    {
+      ROS_ERROR("LaserScannerTrajController:: Could not find mount link:%s", mount_link_name.c_str()) ;
+      track_link_enabled_ = false ;
+    }
+  }
+  else
+  {
+    track_link_enabled_ = false ;
+    ROS_INFO("LaserScannerTrajController:: No longer tracking link") ;
+  }
+
+  track_link_lock_.unlock() ;
+}
+
+
 ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
 LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance()), c_()
 {
@@ -196,8 +258,10 @@
 LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
 {
   node_->unsubscribe(service_prefix_ + "/set_periodic_cmd") ;
+  node_->unsubscribe(service_prefix_ + "/set_track_link_cmd") ;
 
   publisher_->stop() ;
+
   delete publisher_ ;    // Probably should wait on publish_->is_running() before exiting. Need to
                          //   look into shutdown semantics for realtime_publisher
 }
@@ -250,6 +314,7 @@
   }
 
   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) ;
 
   if (publisher_ != NULL)               // Make sure that we don't memory leak if initXml gets called twice
     delete publisher_ ;
@@ -264,3 +329,8 @@
 {
   c_.setPeriodicCmd(cmd_) ;
 }
+
+void LaserScannerTrajControllerNode::setTrackLinkCmd()
+{
+  c_.setTrackLinkCmd(track_link_cmd_) ;
+}

Modified: pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml	2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml	2008-12-17 02:02:47 UTC (rev 8243)
@@ -2,9 +2,9 @@
   <controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
     <max_acc  value="1.0" />
     <max_rate value="100.0" />
-    <filter smoothing_factor="0.1" />
+    <filter smoothing_factor=".1" />
     <joint name="laser_tilt_mount_joint">
-      <pid p="12" i=".1" d="1" iClamp="0.5" />
+      <pid p="2" i=".1" d="0.25" iClamp="0.5" />
     </joint>
   </controller> 
 </controllers>


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