gotoAbsolutePose.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  *
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 
00014 
00015 /* Move a vehicle with a velocity proportional to the distance 
00016  * between the current pose and a desired absolute pose 
00017 */
00018 
00019 #include <stdlib.h>
00020 #include <string.h>
00021 
00022 #include <ros/ros.h>
00023 #include <nav_msgs/Odometry.h>
00024 #include <osg/Quat>
00025 #include <osg/Vec3d>
00026 #include <osg/Matrix>
00027 
00028 bool firstpass=false;
00029 osg::Quat initialQ, goalQ, currentQ;
00030 osg::Vec3d initialT, goalT, currentT;
00031 double totalDistance, currentDistance;
00032 
00033 void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
00034         if (!firstpass) {
00035                 initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
00036                 initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
00037                 totalDistance=(goalT-initialT).length();
00038                 firstpass=true;
00039         } 
00040         currentT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
00041         currentDistance=(goalT-currentT).length();
00042 }
00043 
00044 int main(int argc, char **argv) {
00045 
00046 
00047         if (argc!=9) {
00048                 std::cerr << "USAGE: " << argv[0] << " <vehiclePoseTopic> <vehicleControlTopic> <x> <y> <z> <roll> <pitch> <yaw>" << std::endl;
00049                 std::cerr << "units are meters and radians." << std::endl;
00050                 return 0;
00051         }       
00052 
00053         std::string poseTopic(argv[1]);
00054         std::string controlTopic(argv[2]);
00055         double x=atof(argv[3]);
00056         double y=atof(argv[4]);
00057         double z=atof(argv[5]);
00058         double roll=atof(argv[6]);
00059         double pitch=atof(argv[7]);
00060         double yaw=atof(argv[8]);
00061         
00062         std::string nodeName=controlTopic;
00063         nodeName.replace(0,1,"a");
00064         ros::init(argc, argv, nodeName);
00065         ros::NodeHandle nh;
00066 
00067         osg::Matrixd T, Rx, Ry, Rz, transform;
00068         T.makeTranslate(x,y,z);
00069         Rx.makeRotate(roll,1,0,0);
00070         Ry.makeRotate(pitch,0,1,0);
00071         Rz.makeRotate(yaw,0,0,1);
00072         transform=Rz*Ry*Rx*T;
00073         goalT=transform.getTrans();
00074         goalQ=transform.getRotate();
00075 
00076         ros::Publisher position_pub=nh.advertise<nav_msgs::Odometry>(controlTopic,1);
00077         ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback);
00078 
00079         ros::Rate r(30);
00080         while (ros::ok()) {
00081            if (firstpass) {
00082                 osg::Vec3d vT=(goalT-currentT)*0.15;
00083                 double vScale=(vT.length()>0.1) ? 0.1/vT.length() : 1;
00084 /*
00085                 std::cerr << "Initial Q: " << initialQ.x() << " " << initialQ.y() << " " << initialQ.z() << " " << initialQ.w() << std::endl;
00086                 std::cerr << "Goal Q: " << goalQ.x() << " " << goalQ.y() << " " << goalQ.z() << " " << goalQ.w() << std::endl;
00087                 std::cerr << "Goal Q: " << currentQ.x() << " " << currentQ.y() << " " << currentQ.z() << " " << currentQ.w() << std::endl;
00088                 std::cerr << "Goal T: " << goalT.x() << " " << goalT.y() << " " << goalT.z() << std::endl;
00089                 std::cerr << "Current T: " << currentT.x() << " " << currentT.y() << " " << currentT.z() << std::endl;
00090                 std::cerr << "Goal-Current" << (goalT-currentT).x() << " " << (goalT-currentT).y() << " " << (goalT-currentT).z() << std::endl;
00091                 std::cerr << "Current dist: " << currentDistance << " Total: " << totalDistance << std::endl;
00092                 std::cerr << "current/total: " << currentDistance/totalDistance << std::endl;
00093                 std::cerr << "Slerp: " << 1 - currentDistance/totalDistance << std::endl;
00094                 std::cerr << std::endl;
00095 */
00096                 currentQ.slerp(1-currentDistance/totalDistance,initialQ, goalQ);
00097                 nav_msgs::Odometry odom;
00098                 odom.pose.pose.position.x=currentT.x()+vT.x()*vScale;
00099                 odom.pose.pose.position.y=currentT.y()+vT.y()*vScale;
00100                 odom.pose.pose.position.z=currentT.z()+vT.z()*vScale;
00101                 odom.pose.pose.orientation.x=currentQ.x();
00102                 odom.pose.pose.orientation.y=currentQ.y();
00103                 odom.pose.pose.orientation.z=currentQ.z();
00104                 odom.pose.pose.orientation.w=currentQ.w();
00105 
00106                 odom.twist.twist.linear.x=0;
00107                 odom.twist.twist.linear.y=0;
00108                 odom.twist.twist.linear.z=0;
00109                 odom.twist.twist.angular.x=0;
00110                 odom.twist.twist.angular.y=0;
00111                 odom.twist.twist.angular.z=0;
00112                 for (int i=0; i<36; i++) {
00113                         odom.twist.covariance[i]=0;
00114                         odom.pose.covariance[i]=0;
00115                 }
00116                 position_pub.publish(odom);
00117            }
00118            ros::spinOnce();
00119            r.sleep();
00120         }
00121 
00122         return 0;
00123 }


uwsim
Author(s): Mario Prats , Javier Perez
autogenerated on Fri Aug 28 2015 13:28:58