laser_scanner_traj_controller.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #pragma once
00036 
00037 #include <ros/node_handle.h>
00038 
00039 #include <pr2_controller_interface/controller.h>
00040 #include <control_toolbox/pid.h>
00041 //#include <robot_mechanism_controllers/joint_position_smoothing_controller.h>
00042 
00043 #include <realtime_tools/realtime_publisher.h>
00044 #include <tf/tf.h>
00045 
00046 #include "filters/filter_chain.h"
00047 
00048 // Messages
00049 #include <pr2_msgs/LaserScannerSignal.h>
00050 #include <pr2_msgs/PeriodicCmd.h>
00051 #include <pr2_mechanism_controllers/TrackLinkCmd.h>
00052 #include <pr2_msgs/LaserTrajCmd.h>
00053 
00054 // Services
00055 #include <pr2_mechanism_controllers/SetProfile.h>
00056 #include <pr2_msgs/SetPeriodicCmd.h>
00057 #include <pr2_msgs/SetLaserTrajCmd.h>
00058 
00059 #include "boost/thread/mutex.hpp"
00060 #include <boost/thread/condition.hpp>
00061 #include "pr2_mechanism_controllers/trajectory.h"
00062 
00063 namespace controller
00064 {
00065 
00066 class LaserScannerTrajController
00067 {
00068 public:
00069   LaserScannerTrajController() ;
00070   ~LaserScannerTrajController() ;
00071 
00072   bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00073 
00074   virtual void update() ;
00075 
00076   bool setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd) ;
00077 
00078   bool setTrajCmd(const pr2_msgs::LaserTrajCmd& traj_cmd) ;
00079 
00080   //bool setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
00081 
00082   bool setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
00083                      double max_rate, double max_acc, std::string interp ) ;
00084 
00086   inline double getCurProfileTime() ;
00088   inline double getProfileDuration() ;
00090   inline int getCurProfileSegment() ;
00091 
00092 private:
00093 
00094   pr2_mechanism_model::RobotState *robot_ ;
00095   pr2_mechanism_model::JointState *joint_state_ ;                       // Need this to check the calibrated flag on the joint
00096 
00097   boost::mutex traj_lock_ ;                                             // Mutex for traj_
00098   trajectory::Trajectory traj_ ;                                        // Stores the current trajectory being executed
00099 
00100   tf::Vector3 track_point_ ;
00101 
00102   std::string name_ ;                                                   // The controller name. Used for ROS_INFO Messages
00103 
00104   ros::Time traj_start_time_ ;                                          // The time that the trajectory was started (in seconds)
00105   double traj_duration_ ;                                               // The length of the current profile (in seconds)
00106 
00107 
00108   double max_rate_ ;                                                    // Max allowable rate/velocity
00109   double max_acc_ ;                                                     // Max allowable acceleration
00110 
00111   // Control loop state
00112   control_toolbox::Pid pid_controller_ ;                                // Position PID Controller
00113   filters::FilterChain<double> d_error_filter_chain_;                   // Filter on derivative term of error
00114   ros::Time last_time_ ;                                                // The previous time at which the control loop was run
00115   double last_error_ ;                                                  // Error for the previous time at which the control loop was run
00116   double tracking_offset_ ;                                             // Position cmd generated by the track_link code
00117   double traj_command_ ;                                                // Position cmd generated by the trajectory code
00118 };
00119 
00120 class LaserScannerTrajControllerNode : public pr2_controller_interface::Controller
00121 {
00122 public:
00123   LaserScannerTrajControllerNode() ;
00124   ~LaserScannerTrajControllerNode() ;
00125 
00126   void update() ;
00127 
00128   bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00129 
00130   // Message Callbacks
00131   void setPeriodicCmd(const pr2_msgs::PeriodicCmdConstPtr &cmd);
00132   void setTrajCmd(const pr2_msgs::LaserTrajCmdConstPtr &traj_cmd);
00133   //void setTrackLinkCmd() ;
00134   bool setPeriodicSrv(pr2_msgs::SetPeriodicCmd::Request &req,
00135                       pr2_msgs::SetPeriodicCmd::Response &res);
00136   bool setTrajSrv(pr2_msgs::SetLaserTrajCmd::Request &req,
00137                   pr2_msgs::SetLaserTrajCmd::Response &res);
00138 
00139 
00140 private:
00141   ros::NodeHandle node_;
00142   ros::Subscriber sub_set_periodic_cmd_;
00143   ros::Subscriber sub_set_traj_cmd_;
00144   ros::ServiceServer serve_set_periodic_cmd_;
00145   ros::ServiceServer serve_set_Traj_cmd_;
00146 
00147   LaserScannerTrajController c_ ;
00148   pr2_mechanism_model::RobotState *robot_ ;
00149   std::string service_prefix_ ;
00150 
00151   int prev_profile_segment_ ;                                                    
00152 
00153   pr2_mechanism_controllers::TrackLinkCmd track_link_cmd_ ;
00154 
00155   pr2_msgs::LaserScannerSignal m_scanner_signal_ ;              
00156   bool need_to_send_msg_ ;                                                       
00157   realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal>* publisher_ ; 
00158 };
00159 
00160 }


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52