00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "pr2_doors_common/door_functions.h"
00038 #include "pr2_doors_actions/action_move_base_door.h"
00039
00040
00041 using namespace tf;
00042 using namespace KDL;
00043 using namespace ros;
00044 using namespace std;
00045 using namespace door_handle_detector;
00046 using namespace pr2_doors_common;
00047 using namespace actionlib;
00048
00049 static const string fixed_frame = "odom_combined";
00050 static const double motion_step = 0.01;
00051 static const double update_rate = 10.0;
00052
00053 MoveBaseDoorAction::MoveBaseDoorAction(tf::TransformListener& tf) :
00054 tf_(tf),
00055 costmap_ros_("costmap_move_base_door", tf),
00056 costmap_model_(costmap_),
00057 action_server_(ros::NodeHandle(),
00058 "move_base_door",
00059 boost::bind(&MoveBaseDoorAction::execute, this, _1))
00060 {
00061 costmap_ros_.stop();
00062
00063 ros::NodeHandle node;
00064 base_pub_ = node.advertise<geometry_msgs::Twist>("base_controller/command", 10);
00065
00066 search_pattern_sideways_.push_back(0.0);
00067 search_pattern_sideways_.push_back(-1.0);
00068 search_pattern_sideways_.push_back(1.0);
00069 search_pattern_sideways_.push_back(-1.0);
00070 search_pattern_sideways_.push_back(1.0);
00071
00072 search_pattern_forward_.push_back(1.0);
00073 search_pattern_forward_.push_back(1.0);
00074 search_pattern_forward_.push_back(1.0);
00075 search_pattern_forward_.push_back(0.0);
00076 search_pattern_forward_.push_back(0.0);
00077
00078 footprint_ = costmap_ros_.getRobotFootprint();
00079 };
00080
00081
00082
00083 MoveBaseDoorAction::~MoveBaseDoorAction()
00084 {};
00085
00086
00087
00088 geometry_msgs::Point MoveBaseDoorAction::toPoint(const tf::Vector3& pnt)
00089 {
00090 geometry_msgs::Point res;
00091 res.x = pnt.x();
00092 res.y = pnt.y();
00093 res.z = pnt.z();
00094 return res;
00095 }
00096
00097 geometry_msgs::Vector3 MoveBaseDoorAction::toVector(const tf::Vector3& pnt)
00098 {
00099 geometry_msgs::Vector3 res;
00100 res.x = pnt.x();
00101 res.y = pnt.y();
00102 res.z = pnt.z();
00103 return res;
00104 }
00105
00106
00107
00108 std::vector<geometry_msgs::Point> MoveBaseDoorAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost)
00109 {
00110 double cos_th = cos(theta_cost);
00111 double sin_th = sin(theta_cost);
00112 std::vector<geometry_msgs::Point> oriented_footprint;
00113 for(unsigned int i = 0; i < footprint_.size(); ++i){
00114 geometry_msgs::Point new_pt;
00115 new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th);
00116 new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th);
00117 oriented_footprint.push_back(new_pt);
00118 }
00119 return oriented_footprint;
00120 }
00121
00122
00123
00124 void MoveBaseDoorAction::execute(const door_msgs::DoorGoalConstPtr& goal)
00125 {
00126 ROS_INFO("MoveBaseDoorAction: execute");
00127
00128 costmap_ros_.start();
00129 ros::Duration(3.0).sleep();
00130
00131
00132 door_msgs::Door door;
00133 if (!tf_.waitForTransform(fixed_frame, goal->door.header.frame_id, goal->door.header.stamp, ros::Duration(3.0)) ||
00134 !transformTo(tf_, fixed_frame, goal->door, door, fixed_frame)){
00135 ROS_ERROR("MoveBaseDoorAction: could not transform from %s to %s", fixed_frame.c_str(), goal->door.header.frame_id.c_str());
00136 action_server_.setAborted();
00137 return;
00138 }
00139 tf::Pose end_position = getRobotPose(door, 0.5);
00140 tf::Stamped<tf::Pose> start_position;
00141 costmap_ros_.getRobotPose(start_position);
00142 ROS_DEBUG("MoveBaseDoorAction: current robot pose is %f %f %f", start_position.getOrigin().x(), start_position.getOrigin().y(), start_position.getOrigin().z());
00143 ROS_DEBUG("MoveBaseDoorAction: goal robot pose is %f %f %f", end_position.getOrigin().x(), end_position.getOrigin().y(), end_position.getOrigin().z());
00144
00145
00146 tf::Vector3 motion_direction = (end_position.getOrigin() - start_position.getOrigin()).normalize();
00147 tf::Vector3 search_direction = motion_direction.cross(tf::Vector3(0,0,1)).normalize();
00148 ROS_DEBUG("MoveBaseDoorAction: motion direction: %f %f %f", motion_direction.x(), motion_direction.y(), motion_direction.z());
00149 ROS_DEBUG("MoveBaseDoorAction: search direction: %f %f %f", search_direction.x(), search_direction.y(), search_direction.z());
00150
00151 ros::Rate rate(update_rate);
00152 while (ros::ok() && !action_server_.isPreemptRequested()){
00153
00154 costmap_ros_.getCostmapCopy(costmap_);
00155
00156
00157 tf::Stamped<tf::Pose> current_pose;
00158 costmap_ros_.getRobotPose(current_pose);
00159 tf::Vector3 current_position = current_pose.getOrigin();
00160 double current_orientation = tf::getYaw(current_pose.getRotation());
00161
00162
00163 ROS_DEBUG("MoveBaseDoorAction: distance to goal: %f", motion_direction.dot(end_position.getOrigin() - current_position));
00164 if (motion_direction.dot(end_position.getOrigin() - current_position) < 0){
00165 geometry_msgs::Twist base_twist;
00166 base_pub_.publish(base_twist);
00167 door_msgs::DoorResult result; result.door = door;
00168 action_server_.setSucceeded(result);
00169 costmap_ros_.stop();
00170 ROS_INFO("MoveBaseDoorAction: reached goal");
00171 return;
00172 }
00173
00174
00175 tf::Vector3 next_position;
00176 bool success = false;
00177 for (int i=0; i<(int)search_pattern_sideways_.size(); i++){
00178 next_position = current_position + (motion_direction*motion_step*search_pattern_forward_[i]) + (search_direction * motion_step * search_pattern_sideways_[i]);
00179
00180
00181 if (costmap_model_.footprintCost(toPoint(next_position),
00182 getOrientedFootprint(next_position, current_orientation),
00183 costmap_ros_.getInscribedRadius(),
00184 costmap_ros_.getCircumscribedRadius()) >= 0){
00185 ROS_DEBUG("MoveBaseDoorAction: Valid robot position: %f %f %f", next_position.x(), next_position.y(), next_position.z());
00186 success = true;
00187 break;
00188 }
00189 ROS_DEBUG("MoveBaseDoorAction: IN-Valid robot position: %f %f %f", next_position.x(), next_position.y(), next_position.z());
00190 }
00191 geometry_msgs::Twist base_twist;
00192 if (success){
00193 base_twist.linear = toVector(((current_pose.inverse() * tf::Pose(tf::Quaternion::getIdentity(), next_position)).getOrigin()) * update_rate );
00194
00195 }
00196
00197 base_pub_.publish(base_twist);
00198
00199 rate.sleep();
00200 }
00201
00202 costmap_ros_.stop();
00203 action_server_.setPreempted();
00204 }
00205