ias_drawer_executive: ias_drawer_executive: RobotDriver.cpp Source File

00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ias_drawer_executive/RobotDriver.h>
00031 #include <ias_drawer_executive/RobotArm.h>
00032 #include <ias_drawer_executive/Geometry.h>
00033 
00034 #include <ias_drawer_executive/Poses.h>
00035 
00036 #include <sensor_msgs/point_cloud_conversion.h>
00037 
00038 RobotDriver::RobotDriver()
00039 {
00040     //set up the publisher for the cmd_vel topic
00041     cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/navigation/cmd_vel", 1);
00042     subScan_ = nh_.subscribe("base_scan", 10, &RobotDriver::scanCallback, this);
00043     projector_ = 0;
00044     weHaveScan = false;
00045 }
00046 
00047 RobotDriver *RobotDriver::getInstance()
00048 {
00049     if (!instance)
00050         instance = new RobotDriver();
00051     return instance;
00052 }
00053 
00054 void RobotDriver::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in)
00055 {
00056     if (!projector_)
00057         projector_ = new laser_geometry::LaserProjection();
00058 
00059     scan_mutex.lock();
00060     sensor_msgs::PointCloud scan_cloud;
00061     projector_->projectLaser(*scan_in,scan_cloud);
00062     int step = 1;
00063     for (size_t k = 0; k < scan_cloud.points.size(); k += step)
00064     {
00065         scanPoints[k/step][0] = scan_cloud.points[k].x;
00066         scanPoints[k/step][1] = scan_cloud.points[k].y;
00067         numScanPoints = k / step;
00068         //ROS_INFO("SCAN POINTS : %i %f %f",numScanPoints, scanPoints[k/step][0], scanPoints[k/step][1]);
00069     }
00070     scan_mutex.unlock();
00071     weHaveScan = true;
00072 }
00073 
00074 bool RobotDriver::checkCollision(double relativePose[])
00075 {
00076     ros::Rate rate(10);
00077     while (!weHaveScan)
00078     {
00079         rate.sleep();
00080         ros::spinOnce();
00081     }
00082     scan_mutex.lock();
00083     ROS_INFO("SCAN POINTS : %zu POSE : %f %f",numScanPoints, relativePose[0], relativePose[1] );
00084     bool good = true;
00085     double padding = 0.05;
00086     for (size_t k = 0; good && (k < numScanPoints); k += 1)
00087     {
00088         double x = scanPoints[k][0] + relativePose[0] + 0.275;
00089         double y = scanPoints[k][1] + relativePose[1];
00090         if ((x < .325 + padding) && (x > -.325 - padding) && (y < .325 + padding) && (y > -.325 - padding))
00091         {
00092             //ROS_INFO("POINT %f %f",  scanPoints[k][0] , scanPoints[k][1]);
00093             good = false;
00094         }
00095     }
00096     scan_mutex.unlock();
00097     return good;
00098 }
00099 
00100 bool RobotDriver::checkCollision(tf::Stamped<tf::Pose> target)
00101 {
00102     tf::Stamped<tf::Pose> inBase = Geometry::getPoseIn("base_link",target);
00103     double relativePose[2];
00104     relativePose[0] = -inBase.getOrigin().x();
00105     relativePose[1] = -inBase.getOrigin().y();
00106     return checkCollision(relativePose);
00107 }
00108 
00109 void RobotDriver::getRobotPose(tf::Stamped<tf::Pose> &marker)
00110 {
00111     tf::StampedTransform transform;
00112 
00113     listener_.waitForTransform("map", "base_link",
00114                                ros::Time(0), ros::Duration(30.0));
00115     try
00116     {
00117         listener_.lookupTransform("map", "base_link",ros::Time(0), transform);
00118     }
00119     catch (tf::TransformException ex)
00120     {
00121         ROS_ERROR("%s",ex.what());
00122     }
00123     tf::Stamped<tf::Pose> ret;
00124     ret.frame_id_ = transform.frame_id_;
00125     ret.stamp_ = transform.stamp_;
00126     ret.setOrigin(transform.getOrigin());
00127     ret.setRotation(transform.getRotation());
00128     //marker = transform;
00129     marker = ret;
00130 }
00131 
00132 void RobotDriver::QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler)
00133 {
00134     btScalar W = TQuat.getW();
00135     btScalar X = TQuat.getX();
00136     btScalar Y = TQuat.getY();
00137     btScalar Z = TQuat.getZ();
00138     double WSquared = W * W;
00139     double XSquared = X * X;
00140     double YSquared = Y * Y;
00141     double ZSquared = Z * Z;
00142     TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
00143     TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
00144     TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
00145 //   TEuler *= RADTODEG;
00146 }
00147 
00148 bool RobotDriver::driveInMap(tf::Stamped<tf::Pose> targetPoseMap,bool exitWhenStuck)
00149 {
00150 
00151     tf::Stamped<tf::Pose> targetPoseBase;
00152 
00153     tf::Stamped<tf::Pose> startPose;
00154     getRobotPose(startPose);
00155 
00156     btVector3 diff = targetPoseMap.getOrigin() - startPose.getOrigin();
00157     //we dont move in z
00158     diff = btVector3(diff.x(), diff.y(), 0);
00159     double dist = diff.length();
00160     ROS_INFO("DISTANCE TO TRAVEL %f", dist);
00161 
00162     //if (dist < 0.01)
00163     //return true;
00164 
00165     ROS_INFO("diff unnorm %f %f %f ", diff.x(),diff.y(),diff.z());
00166     if (dist != 0)
00167         diff = (diff * ( 1.0 / dist)) ;
00168     ROS_INFO("diff norm %f %f %f ", diff.x(),diff.y(),diff.z());
00169     btVector3 rel = (dist + 0.05) * diff;
00170     ROS_INFO("new target in base %f %f", rel.x(), rel.y());
00171 
00172     ROS_INFO("TPM before adjust %f %f ", targetPoseMap.getOrigin().x(), targetPoseMap.getOrigin().y());
00173     targetPoseMap.setOrigin(startPose.getOrigin() + (dist + 0.02) * diff);
00174     ROS_INFO("TPM after adjust %f %f", targetPoseMap.getOrigin().x(), targetPoseMap.getOrigin().y());
00175 
00176     bool done = false;
00177     bool doneBad = false;
00178     int cnt = 0;
00179     int cntbad = 0;
00180     double lastDistMoved = 0;
00181 
00182     ROS_INFO("TARGET POSE IN MAP %f %f", targetPoseMap.getOrigin().x(), targetPoseMap.getOrigin().y());
00183 
00184     while (ros::ok() && !done)
00185     {
00186         cnt++;
00187         listener_.waitForTransform("base_link", "map",
00188                                    ros::Time(0), ros::Duration(30.0));
00189 
00190         targetPoseMap.stamp_ = ros::Time();
00191         targetPoseBase.stamp_ = ros::Time();
00192 
00193         listener_.transformPose("base_link", targetPoseMap, targetPoseBase);
00194 
00195         btVector3 euler;
00196         QuaternionToEuler(targetPoseBase.getRotation(),euler);
00197 
00198 
00199         //get the difference between the two poses
00200         //geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
00201         //tf_.transformPose(fixed_frame_, pose, fixed_pose);
00202 
00203         //wait for the listener to get the first message
00204         listener_.waitForTransform("base_footprint", "odom_combined",
00205                                    ros::Time(0), ros::Duration(10.0));
00206 
00207         //we will record transforms here
00208         tf::StampedTransform start_transform;
00209         tf::StampedTransform current_transform;
00210 
00211         //record the starting transform from the odometry to the base frame
00212         listener_.lookupTransform("base_footprint", "odom_combined",
00213                                   ros::Time(0), start_transform);
00214 
00215         //we will be sending commands of type "twist"
00216         geometry_msgs::Twist base_cmd;
00217 
00218         //calulate speeds for reaching goal in 1 second and then scale them down to reasonable speeds
00219         double theoretic_x = targetPoseBase.getOrigin().x();
00220         double theoretic_y = targetPoseBase.getOrigin().y();
00221         double theoretic_w = euler.z();
00222 
00223 
00224 
00225         double scale_x = 1;
00226         double scale_y = 1;
00227         double scale_w = 1;
00228         if (fabs(theoretic_x) > 0.05)
00229             scale_x = 0.05 / fabs(theoretic_x);
00230         if (fabs(theoretic_y) > 0.05)
00231             scale_y = 0.05 / fabs(theoretic_y);
00232         if (fabs(theoretic_w) > 0.25)
00233             scale_w = 0.25 / fabs(theoretic_w);
00234 
00235         double scale = scale_x;
00236         if (scale_y < scale)
00237             scale = scale_y;
00238         if (scale_w < scale)
00239             scale = scale_w;
00240 
00241         //ROS_INFO("scales %f %f %f", scale_x, scale_y, scale_w);
00242 
00243         scale = fabs(scale);
00244         //if (scale > 1)
00245         //  scale = 1;
00246 
00247         base_cmd.linear.x = theoretic_x * scale;
00248         base_cmd.linear.y = theoretic_y * scale;
00249         base_cmd.angular.z = theoretic_w * scale;
00250 
00251         if ((scale == scale_w) && (fabs(theoretic_w) < 0.05))
00252             base_cmd.angular.z = theoretic_w * 0.05 / fabs(theoretic_w);
00253 
00254         //ROS_INFO("COMMAND %f %f %f", base_cmd.linear.x, base_cmd.linear.y, base_cmd.angular.z);
00255 
00256         ros::Rate rate(10.0);
00257 
00258         // if ((base_cmd.angular.z == 0) && (base_cmd.linear.y == 0) && (base_cmd.linear.x == 0))
00259         double distToTarg = sqrt(targetPoseBase.getOrigin().x() * targetPoseBase.getOrigin().x() + targetPoseBase.getOrigin().y() * targetPoseBase.getOrigin().y());
00260         //ROS_INFO("dist %f target pose in base coords: %f %f %f %f ANGLE %f ", distToTarg, targetPoseBase.getOrigin().x(), targetPoseBase.getOrigin().y(), targetPoseBase.getRotation().z(), targetPoseBase.getRotation().w(),euler.z());
00261 
00262         //if ((fabs(targetPoseBase.getOrigin().x()) < 0.02) && (fabs(targetPoseBase.getOrigin().y()) < 0.05) &&  (fabs(euler.z()) < 0.01))
00263         if ((distToTarg < 0.02) && (targetPoseBase.getRotation().getAngle() < 0.05))
00264             done = true;
00265         //  while (!done && nh_.ok())
00266 
00267         //if (distToTarg > 0.3f) {
00268 
00269         tf::Stamped<tf::Pose> actPose;
00270         getRobotPose(actPose);
00271         double trav = (3 * (actPose.getOrigin() - startPose.getOrigin()).length()) + 0.2;
00272         double used = (distToTarg < trav) ? distToTarg : trav;
00273         //double used = distToTarg;
00275         double approach_dist = 0.125; // 0.25;
00276         //if (used > 0.25f) {
00277         // speed up when further away
00278         if (used > approach_dist)
00279         {
00280             //double used = (distToTarg < lastDistMoved) ? distToTarg : lastDistMoved;
00281             //double dscale = (distToTarg + .7) * (distToTarg + .7);
00282             double dscale = (used + (1-approach_dist)) * (used + (1-approach_dist)) * (used + (1-approach_dist));
00283             if (dscale > 5.0)
00284                 dscale = 5.0;
00285             //ROS_INFO("USED %f TRAV %f TOTARG %f SCALE %f", used, trav, distToTarg, dscale);
00286             base_cmd.linear.x *= dscale;
00287             base_cmd.linear.y *= dscale;
00288             base_cmd.angular.z *= dscale;
00289         }
00290         else
00291         {
00292             //ROS_INFO("USED %f TRAV %f TOTARG %f", used, trav, distToTarg);
00293         }
00294 
00295 
00296         //send the drive command after safety check
00297         //if ((fabs(base_cmd.linear.x) <= 0.051) && (fabs(base_cmd.linear.y) <= 0.051) && (fabs(base_cmd.angular.z) <= 0.26))
00298         //if ((fabs(base_cmd.linear.x) <= 0.153) && (fabs(base_cmd.linear.y) <= 0.153) && (fabs(base_cmd.angular.z) <= 0.78))
00299 
00300         if (isnan(base_cmd.linear.x))
00301             base_cmd.linear.x = 0;
00302         if (isnan(base_cmd.linear.y))
00303             base_cmd.linear.y = 0;
00304 
00305         if ((fabs(base_cmd.linear.x) <= 0.255) && (fabs(base_cmd.linear.y) <= 0.255) && (fabs(base_cmd.angular.z) <= 1.3))
00306             cmd_vel_pub_.publish(base_cmd);
00307         else
00308             ROS_ERROR("COMMAND EXCEEDS MAXIMUM LIMITS (COMMAND %f %f %f)", base_cmd.linear.x, base_cmd.linear.y, base_cmd.angular.z);
00309         rate.sleep();
00310         //get the current transform
00311         try
00312         {
00313             listener_.lookupTransform("base_footprint", "odom_combined",
00314                                       ros::Time(0), current_transform);
00315         }
00316         catch (tf::TransformException ex)
00317         {
00318             ROS_ERROR("%s",ex.what());
00319             break;
00320         }
00321         //see how far we've traveled
00322         tf::Transform relative_transform = start_transform.inverse() * current_transform;
00323 
00324         double dist_moved = relative_transform.getOrigin().length() + relative_transform.getRotation().getAngle() * 100.0f;
00325         lastDistMoved = dist_moved;
00326         //ROS_INFO("dist_moved %f", dist_moved);
00327         // if we didnt move much and tried sometimes already, get out, we're stuck
00328         if ((dist_moved < 0.005) && (cnt < 15))
00329             cntbad++;
00330         else
00331             cntbad = 0;
00332 
00333         if (cntbad > 10)
00334         {
00335             ROS_ERROR("RobotDriver: could not reach goal, stuck");
00336 
00337             if (exitWhenStuck)
00338             {
00339                 done = true;
00340                 doneBad = true;
00341             }
00342         }
00343 //      if(dist_moved > distance) done = true;
00344     }
00345 
00346 
00347     tf::Stamped<tf::Pose> actPose;
00348     getRobotPose(actPose);
00349     double travelled = (actPose.getOrigin() - startPose.getOrigin()).length();
00350     //ROS_ERROR("XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX");
00351     btVector3 euler;
00352     ROS_INFO("TRAVELLED %f", travelled);
00353     QuaternionToEuler(targetPoseMap.getRotation(),euler);
00354     ROS_INFO("Goal in Map: %f %f %f", targetPoseMap.getOrigin().x(), targetPoseMap.getOrigin().y(), euler.z());
00355     QuaternionToEuler(actPose.getRotation(),euler);
00356     ROS_INFO("Curr in Map: %f %f %f", actPose.getOrigin().x(), actPose.getOrigin().y(), euler.z());
00357     //if (done) return true;
00358     return true;
00359 }
00360 
00361 
00362 bool RobotDriver::driveInMap(const double targetPose[], bool exitWhenStuck)
00363 {
00364     tf::Stamped<tf::Pose> targetPoseMap;
00365     targetPoseMap.frame_id_ = "map";
00366     targetPoseMap.stamp_ = ros::Time();
00367     targetPoseMap.setOrigin(btVector3( targetPose[0], targetPose[1], 0));
00368     targetPoseMap.setRotation(btQuaternion(0,0, targetPose[2],  targetPose[3]));
00369 
00370     return driveInMap(targetPoseMap,exitWhenStuck);
00371 }
00372 
00373 bool RobotDriver::driveInOdom(const double targetPose[], bool exitWhenStuck)
00374 {
00375 
00376     listener_.waitForTransform("base_link", "map",
00377                                ros::Time(0), ros::Duration(10.0));
00378 
00379     tf::Stamped<tf::Pose> targetPoseMap;
00380     targetPoseMap.frame_id_ = "base_link";
00381     targetPoseMap.stamp_ = ros::Time();
00382     targetPoseMap.setOrigin(btVector3( targetPose[0], targetPose[1], 0));
00383     targetPoseMap.setRotation(btQuaternion(0,0, targetPose[2],  targetPose[3]));
00384 
00385     tf::Stamped<tf::Pose> targetPoseBase;
00386 
00387     targetPoseBase.stamp_ = ros::Time();
00388 
00389     listener_.transformPose("map", targetPoseMap, targetPoseBase);
00390 
00391     double pose[4];
00392     pose[0] = targetPoseBase.getOrigin().x();
00393     pose[1] = targetPoseBase.getOrigin().y();
00394     pose[2] = targetPoseBase.getRotation().z();
00395     pose[3] = targetPoseBase.getRotation().w();
00396 
00397     return driveInMap(pose,exitWhenStuck);
00398 }
00399 
00400 
00401 
00402 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00403 
00404 void RobotDriver::moveBase4(double x, double y, double oz, double ow, bool useNavigation)
00405 {
00406     double p[4];
00407     p[0] = x;
00408     p[1] = y;
00409     p[2] = oz;
00410     p[3] = ow;
00411     ROS_INFO("going to %f %f %f %f", p[0], p[1], p[2], p[3]);
00412     RobotDriver::getInstance()->moveBase(p);
00413 }
00414 
00415 void RobotDriver::moveBase(const double pose[], bool useNavigation)
00416 {
00417     double x = pose[0];
00418     double y = pose[1];
00419     double oz = pose[2];
00420     double ow = pose[3];
00421 
00422     //RobotArm *arm = RobotArm::getInstance();
00423     // Start the trajectory
00424 
00425     //if (!arm->isTucked())
00426     //{
00427     //arm->startTrajectory(arm->twoPointTrajectory(Poses::untuckPoseB, Poses::untuckPoseB));
00429     //}
00430 
00431     //tell the action client that we want to spin a thread by default
00432     MoveBaseClient ac("move_base", true);
00433 
00434     //wait for the action server to come up
00435     //while(ros::ok() && !ac.waitForServer(ros::Duration(5.0))){
00436     //  ROS_INFO("Waiting for the move_base action server to come up");
00437     //}
00438 
00439     move_base_msgs::MoveBaseGoal goal;
00440 
00441     //we'll send a goal to the robot to move 1 meter forward
00442     goal.target_pose.header.frame_id = "/map";
00443     goal.target_pose.header.stamp = ros::Time::now();
00444 
00445     goal.target_pose.pose.position.x = x;
00446     goal.target_pose.pose.position.y = y;
00447     goal.target_pose.pose.orientation.z = oz;
00448     goal.target_pose.pose.orientation.w = ow;
00449 
00450     bool use_move_base = useNavigation;
00451 
00452     if (use_move_base)
00453     {
00454         ROS_INFO("Sending goal");
00455         ac.sendGoal(goal);
00456 
00457         //bool finished_before_timeout =
00458         ac.waitForResult(ros::Duration(30.0));
00459 
00460         if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00461             ROS_INFO("Hooray, the base moved");
00462         else
00463             ROS_INFO("The base failed to movefor some reason");
00464     }
00465 
00466     RobotDriver *driver = RobotDriver::getInstance();
00467 
00468     driver->driveInMap(pose);
00469 }
00470 
00471 void RobotDriver::moveBaseP(double x, double y, double oz, double ow, bool useNavigation)
00472 {
00473     double p[4];
00474     p[0] = x;
00475     p[1] = y;
00476     p[2] = oz;
00477     p[3] = ow;
00478     moveBase(p,useNavigation);
00479 }
00480 
00481 void RobotDriver::driveToMatch(std::vector<tf::Stamped<tf::Pose> > targetPose, std::vector<std::string> frame_ids)
00482 {
00483     ros::Rate rate(5);
00484     while (ros::ok())
00485     {
00486 
00487         rate.sleep();
00488         ros::spinOnce();
00489 
00490         RobotArm *right = RobotArm::getInstance(0);
00491         RobotArm *left = RobotArm::getInstance(1);
00492         tf::Stamped<tf::Pose> rightToolPose;
00493         right->getToolPose(rightToolPose, "base_link");
00494         tf::Stamped<tf::Pose> leftToolPose;
00495         left->getToolPose(leftToolPose, "base_link");
00496 
00497 
00498         tf::Stamped<tf::Pose> goalPoseRight;
00499         goalPoseRight.stamp_ = ros::Time::now();
00500         goalPoseRight.frame_id_= "map";
00501         goalPoseRight.setOrigin(btVector3(-1.657, 1.727, 0.982));
00502         goalPoseRight.setRotation(btQuaternion(-0.618, -0.312, 0.674, 0.256));
00503         goalPoseRight = Geometry::getPoseIn("base_link",goalPoseRight);
00504 
00505         tf::Stamped<tf::Pose> goalPoseLeft;
00506         goalPoseLeft.stamp_ = ros::Time::now();
00507         goalPoseLeft.frame_id_= "map";
00508         goalPoseLeft.setOrigin(btVector3(-1.679, 1.987, 0.971));
00509         goalPoseLeft.setRotation(btQuaternion(-0.285, 0.654, -0.282, 0.641));
00510         goalPoseLeft = Geometry::getPoseIn("base_link",goalPoseLeft);
00511 
00512 
00513         /*- Translation: [-1.657, 1.727, 0.982]
00514         - Rotation: in Quaternion [-0.618, -0.312, 0.674, 0.256]
00515             in RPY [-1.516, 0.739, 1.713]
00516         ruehr@satie:~/sandbox/tumros-internal/highlevel/ias_drawer_executive$ rosrun tf tf_echo map r_gripper_tool_frame
00517         At time 1286556189.961
00518         - Translation: [-1.679, 1.987, 0.971]
00519         - Rotation: in Quaternion [-0.285, 0.654, -0.282, 0.641]
00520             in RPY [-1.597, 0.746, -1.592]
00521         At time 1286556191.008*/
00522 
00523 
00524         //tf::Stamped<tf::Pose> rightTarget = targetPose[1];
00525         //Stamped<tf::Pose> leftTarget = targetPose[1];
00526 
00527         //::Transform relative_transform = start_transform/inverse() * current_transform;
00528         tf::Pose relative_transform_left;// = goalPoseLeft.inverse() * rightToolPose;
00529         tf::Pose relative_transform_right;// = goalPoseRight.inverse() * leftToolPose;
00530 
00531         relative_transform_right.setOrigin(rightToolPose.getOrigin() - goalPoseLeft.getOrigin());
00532         relative_transform_left.setOrigin(leftToolPose.getOrigin() - goalPoseRight.getOrigin());
00533 
00534         btVector3 average = relative_transform_left.getOrigin() + relative_transform_right.getOrigin();
00535 
00536         ROS_INFO("RELATIVE LEFT %f %f %f  %f %f %f %f", relative_transform_left.getOrigin().x(),relative_transform_left.getOrigin().y(),relative_transform_left.getOrigin().z(),
00537                  relative_transform_left.getRotation().x(), relative_transform_left.getRotation().y(), relative_transform_left.getRotation().z(), relative_transform_left.getRotation().w());
00538         ROS_INFO("RELATIVE RIGHT %f %f %f  %f %f %f %f", relative_transform_right.getOrigin().x(),relative_transform_right.getOrigin().y(),relative_transform_right.getOrigin().z(),
00539                  relative_transform_right.getRotation().x(), relative_transform_right.getRotation().y(), relative_transform_right.getRotation().z(), relative_transform_right.getRotation().w());
00540 
00541         ROS_INFO("AVERAGE %f %f", average.x(), average.y());
00542     }
00543 
00544 }
00545 
00546 RobotDriver *RobotDriver::instance = 0;
00547 
00548 
00549