head_position_controller.cpp
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 
00036 #include "joint_qualification_controllers/head_position_controller.h"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 
00040 
00041 PLUGINLIB_DECLARE_CLASS(joint_qualification_controllers, HeadPositionController, 
00042                         joint_qualification_controllers::HeadPositionController, 
00043                         pr2_controller_interface::Controller)
00044 
00045 using namespace std;
00046 using namespace joint_qualification_controllers;
00047 
00048 HeadPositionController::HeadPositionController()
00049   : robot_state_(NULL)
00050 {}
00051 
00052 HeadPositionController::~HeadPositionController()
00053 {
00054   sub_command_.shutdown();
00055 }
00056 
00057 bool HeadPositionController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
00058 {
00059   node_ = n;
00060 
00061 // get name link names from the param server
00062   if (!node_.getParam("pan_link_name", pan_link_name_)){
00063     ROS_ERROR("HeadPositionController: No pan link name found on parameter server (namespace: %s)",
00064               node_.getNamespace().c_str());
00065     return false;
00066   }
00067   if (!node_.getParam("tilt_link_name", tilt_link_name_)){
00068     ROS_ERROR("HeadPositionController: No tilt link name found on parameter server (namespace: %s)",
00069               node_.getNamespace().c_str());
00070     return false;
00071   }
00072 
00073   // test if we got robot pointer
00074   assert(robot_state);
00075   robot_state_ = robot_state;
00076 
00077   //initialize the joint position controllers
00078 
00079   ros::NodeHandle nh_pan(node_, "pan_controller");
00080   ros::NodeHandle nh_tilt(node_, "tilt_controller");
00081   head_pan_controller_.init(robot_state, nh_pan);
00082   head_tilt_controller_.init(robot_state, nh_tilt);
00083 
00084 
00085   // subscribe to head commands
00086   sub_command_ = node_.subscribe<sensor_msgs::JointState>("command", 1, &HeadPositionController::command, this);
00087 
00088   return true;
00089 }
00090 
00091 void HeadPositionController::starting()
00092 {
00093   pan_out_ = head_pan_controller_.joint_state_->position_;
00094   tilt_out_ = head_tilt_controller_.joint_state_->position_;
00095   head_pan_controller_.starting();
00096   head_tilt_controller_.starting();
00097 }
00098 
00099 void HeadPositionController::update()
00100 {
00101   // set position controller commands
00102   head_pan_controller_.command_ = pan_out_;
00103   head_tilt_controller_.command_ = tilt_out_;
00104   head_pan_controller_.update();
00105   head_tilt_controller_.update();
00106 }
00107 
00108 void HeadPositionController::command(const sensor_msgs::JointStateConstPtr& command_msg)
00109 {
00110   // do not use assert to check user input!
00111 
00112   if (command_msg->name.size() != 2 || command_msg->position.size() != 2){
00113     ROS_ERROR("Head servoing controller expected joint command of size 2");
00114     return;
00115   }
00116   if (command_msg->name[0] == head_pan_controller_.joint_state_->joint_->name &&
00117       command_msg->name[1] == head_tilt_controller_.joint_state_->joint_->name)
00118   {
00119     pan_out_  = command_msg->position[0];
00120     tilt_out_ = command_msg->position[1];
00121   }
00122   else if (command_msg->name[1] == head_pan_controller_.joint_state_->joint_->name &&
00123            command_msg->name[0] == head_tilt_controller_.joint_state_->joint_->name)
00124   {
00125     pan_out_ = command_msg->position[1];
00126     tilt_out_ = command_msg->position[0];
00127   }
00128   else
00129   {
00130     ROS_ERROR("Head servoing controller received invalid joint command");
00131   }
00132 }
00133 
00134 
00135 


joint_qualification_controllers
Author(s): Kevin Watts, Melonee Wise
autogenerated on Sat Dec 28 2013 17:29:55