cart_state_estimator: cart_state_estimator: cart_state

00001 
00002 /*
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  *
00030  */
00031 
00032 #include "cart_state_estimator/cart_state_estimator.h"
00033 #include "cart_state_estimator/util.h"
00034 
00035 namespace cart_state_estimator
00036 {
00037 const string CART_FRAME("cart");
00038 const string BASE_FRAME("base_footprint");
00039 const string CHECKERBOARD_FRAME("CRT");
00040 
00041 /************************************************************
00042  * Initialization
00043  ************************************************************/
00044 
00045 gm::PolygonStamped computeFootprint (const double w, const double l)
00046 {
00047   gm::PolygonStamped footprint;
00048   footprint.header.frame_id = CART_FRAME;
00049   footprint.polygon.points.resize(4);
00050   footprint.polygon.points[1].x = l;
00051   footprint.polygon.points[2].x = l;
00052   footprint.polygon.points[2].y = w;
00053   footprint.polygon.points[3].y = w;
00054   return footprint;
00055 }
00056 
00057 string toString (const btTransform& tr)
00058 {
00059   btVector3 t = tr.getOrigin();
00060   btQuaternion r = tr.getRotation();
00061   return (format("((%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f))") %
00062           t.x() % t.y() % t.z() % r.x() % r.y() % r.z() % r.w()).str();
00063 }
00064 
00065 CartStateEstimator::CartStateEstimator () :
00066   cart_width_(getParam<double>("width", "cart_pushing")),
00067   cart_length_(getParam<double>("length", "cart_pushing")),
00068   l_gripper_frame_(getParam<string>("l_gripper_frame")),
00069   r_gripper_frame_(getParam<string>("r_gripper_frame")),
00070   publish_fake_transforms_(getPrivateParam<bool>("publish_fake_gripper_transforms", false)),
00071   footprint_(computeFootprint(cart_width_, cart_length_)),
00072   footprint_x_offset_(getParam<double>("footprint_x_offset", "cart_pushing")),
00073   footprint_y_offset_(getParam<double>("footprint_y_offset", "cart_pushing")),
00074   publication_interval_(getPrivateParam<double>("publication_interval", 0.05)),
00075   transform_recency_threshold_(getPrivateParam<double>("transform_recency_threshold", 0.4)),
00076   cart_to_board_(readTransformParameter(string("cart_pushing/checkerboard_pose")).inverse()),
00077   pub_timer_(nh_.createWallTimer(WallDuration(publication_interval_), &CartStateEstimator::publishState, this)),
00078   footprint_pub_(nh_.advertise<gm::PolygonStamped> ("cart_footprint", 10)),
00079   cart_init_pose_(readTransformParameter(string("cart_init_pose")))
00080 {
00081     reset_est_server_ = nh_.advertiseService("cart_pushing/reset_estimate", &CartStateEstimator::resetEstimate, this);
00082 
00083     /* Uncomment to use local grasp_solver_ call rather than service call */
00084         gripper_init_poses_.push_back(readTransformParameter(string("r_gripper_grasp")));
00085         gripper_init_poses_.push_back(readTransformParameter(string("l_gripper_grasp")));
00086         grasp_solver_ = ManipulationTransforms(cart_init_pose_, gripper_init_poses_);
00087         ROS_ASSERT (cart_width_ > 0.0 && cart_length_ > 0.0);
00088 }
00089 
00090 bool CartStateEstimator::resetEstimate(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp) {
00091     ROS_INFO("Cart position estimate reset by service call");
00092     Lock l(mutex_);
00093     tf::StampedTransform xfm1, xfm2;
00094     try {
00095         tf_listener_.lookupTransform (BASE_FRAME, l_gripper_frame_, ros::Time(), xfm1);
00096         tf_listener_.lookupTransform (BASE_FRAME, r_gripper_frame_, ros::Time(), xfm2);
00097     }
00098     catch (tf::TransformException ex) {
00099         ROS_ERROR("Transform exception attempting to set initial cart pose: %s", ex.what());
00100         return false;
00101     }
00102 
00103     grasp_solver_.setInitialTransforms(cart_init_pose_, xfm1, xfm2);
00104     return true;
00105 }
00106 
00107 
00108 // Returns cart polygon in base footprint frame
00109 gm::PolygonStamped CartStateEstimator::getProjectedFootprint () const 
00110 {
00111   gm::PolygonStamped footprint = footprint_;
00112   tf::StampedTransform cart_to_base;
00113   tf_listener_.lookupTransform (BASE_FRAME, CART_FRAME, Time(), cart_to_base);
00114   for (unsigned i = 0; i < 4; i++) {
00115                 btVector3 p(footprint.polygon.points[i].x - footprint_x_offset_,
00116                                 footprint.polygon.points[i].y - footprint_y_offset_,
00117                                 footprint.polygon.points[i].z);
00118                 btVector3 trans = cart_to_base * p;
00119                 footprint.polygon.points[i].x = trans.x();
00120                 footprint.polygon.points[i].y = trans.y();
00121                 footprint.polygon.points[i].z = 0.0;
00122         }
00123   footprint.header.stamp = Time::now();
00124   footprint.header.frame_id = BASE_FRAME;
00125   return footprint;
00126 }
00127 
00128 
00129 void CartStateEstimator::broadcastCartPose(const string& frame, const btTransform& trans)
00130 {
00131   tf::StampedTransform stamped(trans, ros::Time::now(), BASE_FRAME, frame);
00132   ROS_DEBUG_STREAM_NAMED("cart_state_estimator", "broadcasting cart pose: " << toString(trans));
00133   tf_broadcaster_.sendTransform(stamped);
00134 }
00135 
00136 void CartStateEstimator::publishState (const WallTimerEvent& e) 
00137 {
00138   try {
00139     footprint_pub_.publish(getProjectedFootprint());
00140   }
00141   catch (tf::TransformException &ex) {
00142     ROS_DEBUG_STREAM_NAMED ("transforms", "Not publishing footprint due to transform exception " << ex.what());
00143   }
00144 
00145   //ROS_DEBUG_NAMED ("node", "Acquiring lock");
00146   Lock l(mutex_);
00147 
00148   /****************************************
00149    * Use gripper positions to get
00150    * proprioceptive estimate
00151    ****************************************/
00152 
00153   optional<btTransform> proprioceptive_estimate;
00154   tf::StampedTransform l_gripper_to_base, r_gripper_to_base;
00155   /* Uncomment to use local grasp_solver_ call rather than service call */
00156   std::vector<tf::Transform> gripper_tfT_base; gripper_tfT_base.resize(2); // Can't be StampedTransform b/c vector clobbers type, and can't be passed
00157   std::vector<gm::PoseStamped> gripper_psT_base; gripper_psT_base.resize(2);
00158   const Time most_recent;
00159 
00160   Time cutoff;
00161   try {
00162     cutoff = Time::now() - Duration(transform_recency_threshold_);
00163   } catch (std::runtime_error &e){
00164     cutoff = Time::now();
00165   }
00166 
00167   try {
00168       // Find gripper poses in base link
00169     tf_listener_.lookupTransform (BASE_FRAME, l_gripper_frame_, most_recent, l_gripper_to_base);
00170     tf_listener_.lookupTransform (BASE_FRAME, r_gripper_frame_, most_recent, r_gripper_to_base);
00171     if (l_gripper_to_base.stamp_ > cutoff && r_gripper_to_base.stamp_ > cutoff) {
00172       btTransform estimate;
00173       ROS_DEBUG_STREAM ("R gripper pose is " << toString(r_gripper_to_base) <<
00174                        " l gripper is " << toString(l_gripper_to_base));
00175 
00176       // Compute object pose using solver directly
00177       /* Uncomment to use local grasp_solver_ call rather than service call */
00178       gripper_tfT_base[0] = btTransform(r_gripper_to_base);
00179       gripper_tfT_base[1] = btTransform(l_gripper_to_base);
00180       grasp_solver_.mapEffectorPosesToObject(gripper_tfT_base, estimate);
00181 
00182       proprioceptive_estimate = estimate;
00183       broadcastCartPose("cart_proprioceptive", *proprioceptive_estimate);
00184     }
00185     else
00186       ROS_DEBUG_STREAM_NAMED ("transforms", "Transforms at " << l_gripper_to_base.stamp_ <<
00187                               " and " << r_gripper_to_base.stamp_ << " are not within cutoff " << cutoff);
00188   }
00189   catch (tf::TransformException &ex) {
00190     ROS_DEBUG_STREAM_NAMED ("transforms", "Skipping proprioceptive estimate due to transform error " << ex.what());
00191   }
00192 
00193   if (proprioceptive_estimate) {
00194     ROS_DEBUG_NAMED ("cart_pose_estimate", "Using proprioceptive estimate");
00195     broadcastCartPose(CART_FRAME, *proprioceptive_estimate);
00196   }
00197   else
00198     ROS_DEBUG_NAMED ("transforms", "Not publishing cart pose as proprioceptive estimate was available");
00199 }
00200 
00201 } // namespace cart_state_estimator
00202 
00203 int main (int argc, char** argv)
00204 {
00205   ros::init(argc, argv, "cart_state_estimator");
00206   cart_state_estimator::CartStateEstimator node;
00207   ros::MultiThreadedSpinner spinner(2); // Because the pub thread sometimes waits for the fake pub thread
00208   spinner.spin();
00209   return 0;
00210 }
cart_state_estimator.cpp
