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 }