move_base_omnidirectional.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/move_base_omnidirectional.h"
00038 
00039 using namespace ros;
00040 using namespace std;
00041 
00042 static const string fixed_frame = "odom_combined";
00043 
00044 namespace pr2_plugs_actions{
00045 
00046 MoveBaseOmnidirectionalAction::MoveBaseOmnidirectionalAction() :
00047   costmap_ros_("costmap_move_base", tf_),
00048   costmap_model_(costmap_),
00049   action_server_(ros::NodeHandle(), 
00050                  "move_base_omnidirectional", 
00051                  boost::bind(&MoveBaseOmnidirectionalAction::execute, this, _1),
00052                  true)
00053 {
00054   costmap_ros_.stop();
00055 
00056   ros::NodeHandle node_private("~");
00057   node_private.param("k_trans", K_trans, 1.0);
00058   node_private.param("k_rot", K_rot, 1.0);
00059   node_private.param("tolerance_trans", tolerance_trans, 0.02);
00060   node_private.param("tolerance_rot", tolerance_rot, 0.04);
00061 
00062   ros::NodeHandle node;
00063   base_pub_ = node.advertise<geometry_msgs::Twist>("base_controller/command", 10);
00064 
00065   footprint_ = costmap_ros_.getRobotFootprint();
00066 };
00067 
00068 
00069 
00070 MoveBaseOmnidirectionalAction::~MoveBaseOmnidirectionalAction()
00071 {};
00072 
00073 
00074 
00075 void MoveBaseOmnidirectionalAction::execute(const move_base_msgs::MoveBaseGoalConstPtr& goal)
00076 { 
00077   ROS_INFO("MoveBaseOmnidirectionalAction: execute");
00078   costmap_ros_.start();
00079 
00080   // get current robot pose
00081   tf::Stamped<tf::Pose> robot_pose;
00082   costmap_ros_.getRobotPose(robot_pose);
00083   ROS_INFO("MoveBaseOmnidirectionalAction: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
00084 
00085   // get desired robot pose
00086   tf::Stamped<tf::Pose> desired_pose;
00087   tf::poseStampedMsgToTF(goal->target_pose, desired_pose);
00088   if (!tf_.waitForTransform(fixed_frame, desired_pose.frame_id_, desired_pose.stamp_, ros::Duration(2.0))){
00089     ROS_ERROR("MoveBaseOmnidirectionalAction: could not transform from %s to %s", fixed_frame.c_str(), desired_pose.frame_id_.c_str());
00090     action_server_.setAborted();
00091     return;
00092   }
00093   tf_.transformPose(fixed_frame, desired_pose, desired_pose);
00094   ROS_INFO("MoveBaseOmnidirectionalAction: desired robot pose %f %f ==> %f", desired_pose.getOrigin().x(), desired_pose.getOrigin().y(), tf::getYaw(desired_pose.getRotation()));
00095 
00096   // command base to desired pose
00097   geometry_msgs::Twist diff = diff2D(desired_pose, robot_pose);
00098   ROS_INFO("MoveBaseOmnidirectionalAction: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
00099   ROS_INFO("MoveBaseOmnidirectionalAction: diff limit %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
00100   ros::Time goal_reached_time = ros::Time::now();
00101   while (goal_reached_time + ros::Duration(0.5) > ros::Time::now()) {
00102     diff = diff2D(desired_pose, robot_pose);
00103     ROS_DEBUG("Angular error: %f", fabs(diff.angular.z));
00104     // check for bounds
00105     if (fabs(diff.linear.x) > tolerance_trans || fabs(diff.linear.y) > tolerance_trans || fabs(diff.angular.z) > tolerance_rot)
00106       goal_reached_time = ros::Time::now();
00107     // check for preemption
00108     if (action_server_.isPreemptRequested()){
00109       ROS_WARN("MoveBaseOmnidirectionalAction: Preempted");
00110       lockWheels();
00111       action_server_.setPreempted();
00112       return;
00113     }
00114     base_pub_.publish(limitTwist(diff));
00115     costmap_ros_.getRobotPose(robot_pose);
00116     ros::Duration(0.01).sleep();
00117   }
00118   costmap_ros_.stop();
00119   lockWheels();
00120   action_server_.setSucceeded();
00121 }
00122 
00123 
00124 
00125 void MoveBaseOmnidirectionalAction::lockWheels()
00126 {
00127   ROS_INFO("MoveBaseOmnidirectionalAction: Locking wheels sideways");
00128   geometry_msgs::Twist twist;
00129   twist.linear.y = -0.001;
00130   base_pub_.publish(twist);
00131   ros::Duration(0.5).sleep();
00132   twist.linear.y = 0.0;
00133   base_pub_.publish(twist);
00134 }
00135 
00136 geometry_msgs::Twist MoveBaseOmnidirectionalAction::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
00137 {
00138   geometry_msgs::Twist res;
00139   tf::Pose diff = pose2.inverse() * pose1;
00140   res.linear.x = diff.getOrigin().x();
00141   res.linear.y = diff.getOrigin().y();
00142   res.angular.z = tf::getYaw(diff.getRotation());
00143   return res;
00144 }
00145 
00146 
00147 geometry_msgs::Twist MoveBaseOmnidirectionalAction::limitTwist(const geometry_msgs::Twist& twist)
00148 {
00149   geometry_msgs::Twist res = twist;
00150   res.linear.x *= K_trans;
00151   res.linear.y *= K_trans;
00152   res.angular.z *= K_rot;
00153 
00154   if (fabs(res.linear.x) > 0.1) res.linear.x = 0.1 * res.linear.x / fabs(res.linear.x);
00155   if (fabs(res.linear.y) > 0.1) res.linear.y = 0.1 * res.linear.y / fabs(res.linear.y);
00156   if (fabs(res.angular.z) > 0.2) res.angular.z = 0.2 * res.angular.z / fabs(res.angular.z);
00157 
00158   ROS_DEBUG("Angular command %f", res.angular.z);
00159   return res;
00160 }
00161 
00162 
00163 
00164 std::vector<geometry_msgs::Point> MoveBaseOmnidirectionalAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost)
00165 {
00166   double cos_th = cos(theta_cost);
00167   double sin_th = sin(theta_cost);
00168   std::vector<geometry_msgs::Point> oriented_footprint;
00169   for(unsigned int i = 0; i < footprint_.size(); ++i){
00170     geometry_msgs::Point new_pt;
00171     new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th);
00172     new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th);
00173     oriented_footprint.push_back(new_pt);
00174   }
00175   return oriented_footprint;
00176 }
00177 
00178 
00179 }
00180 
00181 
00182 
00183 int main(int argc, char** argv)
00184 {
00185   ros::init(argc, argv, "move_base_omnidirectional");
00186 
00187   pr2_plugs_actions::MoveBaseOmnidirectionalAction action_server;
00188 
00189   ros::spin();
00190   return 0;
00191 }


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