align_base.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 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 /* Author: Wim Meeussen */
00036 
00037 #include "pr2_plugs_actions/align_base.h"
00038 
00039 using namespace ros;
00040 using namespace std;
00041 
00042 static const string fixed_frame = "odom_combined";
00043 //static const double desired_distance = 0.81;
00044 
00045 namespace pr2_plugs_actions{
00046 
00047 AlignBaseAction::AlignBaseAction() :
00048   wall_detector_("detect_wall_norm", true),
00049   move_base_omnidirectional_("move_base_omnidirectional", true),
00050   action_server_(ros::NodeHandle(), 
00051                  "align_base", 
00052                  boost::bind(&AlignBaseAction::execute, this, _1),
00053                  true)
00054 {
00055   ROS_INFO("Waiting for action servers");
00056   wall_detector_.waitForServer();
00057   move_base_omnidirectional_.waitForServer();
00058 };
00059 
00060 
00061 AlignBaseAction::~AlignBaseAction()
00062 {};
00063 
00064 
00065 void AlignBaseAction::execute(const pr2_plugs_msgs::AlignBaseGoalConstPtr& goal)
00066 { 
00067   ROS_INFO("AlignBaseAction: execute");
00068 
00069   // get wall normal
00070   pr2_plugs_msgs::DetectWallNormGoal wall_norm_goal;
00071   wall_norm_goal.look_point = goal->look_point;
00072   wall_norm_goal.look_point.point.y = -fabs(goal->desired_distance);
00073   while (ros::ok() && wall_detector_.sendGoalAndWait(wall_norm_goal, ros::Duration(100.0), ros::Duration(5.0)) != actionlib::SimpleClientGoalState::SUCCEEDED)
00074     ROS_INFO("AlignBaseAction: try again to get wall norm");
00075   
00076   // convert wall norm to fixed frame
00077   geometry_msgs::PointStamped wall_point_msg = wall_detector_.getResult()->wall_point;
00078   geometry_msgs::Vector3Stamped wall_norm_msg = wall_detector_.getResult()->wall_norm;
00079   if (!tf_.waitForTransform(fixed_frame, wall_norm_msg.header.frame_id, wall_norm_msg.header.stamp, ros::Duration(2.0))){
00080     ROS_ERROR("AlignBaseAction: failed to transform from frame %s to %s at time %f", fixed_frame.c_str(), wall_norm_msg.header.frame_id.c_str(), wall_norm_msg.header.stamp.toSec());
00081     action_server_.setAborted();
00082     return;
00083   }
00084   tf_.transformPoint(fixed_frame, wall_point_msg, wall_point_msg);
00085   tf_.transformVector(fixed_frame, wall_norm_msg, wall_norm_msg);
00086   tf::Vector3 wall_norm = fromVector(wall_norm_msg.vector);
00087   tf::Vector3 wall_point = fromPoint(wall_point_msg.point);
00088   ROS_INFO("AlignBaseAction: wall norm  %f %f %f", wall_norm.x(), wall_norm.y(), wall_norm.z());
00089   ROS_INFO("AlignBaseAction: wall point %f %f %f", wall_point.x(), wall_point.y(), wall_point.z());
00090 
00091   // check if this is wall
00092   if (fabs(wall_norm.z()) > 0.3){
00093     ROS_ERROR("A wall should be vertical. This vector (%f, %f, %f) is not vertical", wall_norm.x(), wall_norm.y(), wall_norm.z());
00094     action_server_.setAborted();
00095     return;
00096   }
00097   
00098   // get current robot pose
00099   tf::StampedTransform robot_pose;
00100   if (!tf_.waitForTransform(fixed_frame, "base_link", ros::Time(), ros::Duration(2.0))){
00101     ROS_ERROR("AlignBaseAction: failed to transform from frame %s to base_link", fixed_frame.c_str());
00102     action_server_.setAborted();
00103     return;
00104   }
00105   tf_.lookupTransform(fixed_frame, "base_link", ros::Time(), robot_pose);
00106   ROS_INFO("AlignBaseAction: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
00107   
00108   // get desired robot pose
00109   tf::Pose desired_pose;
00110   desired_pose.setOrigin(robot_pose.getOrigin() + (wall_norm * (wall_norm.dot(wall_point-robot_pose.getOrigin()) - goal->desired_distance)));
00111   desired_pose.setRotation(tf::createQuaternionFromYaw(getVectorAngle(tf::Vector3(0,1,0), wall_norm*-1)));
00112   desired_pose = desired_pose * tf::Pose(tf::Quaternion::getIdentity(), tf::Vector3(goal->offset, 0.0, 0.0));
00113   ROS_INFO("AlignBaseAction: desired robot pose %f %f ==> %f", desired_pose.getOrigin().x(), desired_pose.getOrigin().y(), tf::getYaw(desired_pose.getRotation()));
00114     
00115   // command base to desired pose
00116   move_base_msgs::MoveBaseGoal move_base_goal;
00117   tf::poseStampedTFToMsg(tf::Stamped<tf::Pose>(desired_pose, ros::Time::now(), fixed_frame), move_base_goal.target_pose);
00118   if (move_base_omnidirectional_.sendGoalAndWait(move_base_goal, ros::Duration(20.0), ros::Duration(5.0)) != actionlib::SimpleClientGoalState::SUCCEEDED){
00119     ROS_ERROR("AlignBaseAction: failed to move base to desired pose");
00120     action_server_.setAborted();
00121     return;
00122   }
00123   pr2_plugs_msgs::AlignBaseResult  align_result;
00124   align_result.base_pose = move_base_goal.target_pose;
00125   align_result.wall_norm = wall_norm_msg;
00126   action_server_.setSucceeded(align_result);
00127 }
00128 
00129 
00130 
00131 geometry_msgs::Point AlignBaseAction::toPoint(const tf::Vector3& pnt)
00132 {
00133   geometry_msgs::Point res;
00134   res.x = pnt.x();
00135   res.y = pnt.y();
00136   res.z = pnt.z();
00137   return res;
00138 }
00139 
00140 geometry_msgs::Vector3 AlignBaseAction::toVector(const tf::Vector3& pnt)
00141 {
00142   geometry_msgs::Vector3 res;
00143   res.x = pnt.x();
00144   res.y = pnt.y();
00145   res.z = pnt.z();
00146   return res;
00147 }
00148 
00149 tf::Vector3 AlignBaseAction::fromVector(const geometry_msgs::Vector3& pnt)
00150 {
00151   return tf::Vector3(pnt.x, pnt.y, pnt.z);
00152 }
00153 
00154 tf::Vector3 AlignBaseAction::fromPoint(const geometry_msgs::Point& pnt)
00155 {
00156   return tf::Vector3(pnt.x, pnt.y, pnt.z);
00157 }
00158 
00159 
00160 double AlignBaseAction::getVectorAngle(const tf::Vector3& v1, const tf::Vector3& v2)
00161 {
00162   tf::Vector3 vec1 = v1; vec1 = vec1.normalize();
00163   tf::Vector3 vec2 = v2; vec2 = vec2.normalize();
00164   double dot      = vec2.x() * vec1.x() + vec2.y() * vec1.y();
00165   double perp_dot = vec2.y() * vec1.x() - vec2.x() * vec1.y();
00166   return atan2(perp_dot, dot);
00167 }
00168 
00169 }
00170 
00171 
00172 
00173 int main(int argc, char** argv)
00174 {
00175   ros::init(argc, argv, "align_base");
00176 
00177   pr2_plugs_actions::AlignBaseAction action_server;
00178 
00179   ros::spin();
00180   return 0;
00181 }


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13