gotoRelativePose.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 /* Move a vehicle with a velocity proportional to the distance 
00015  * between the current pose and a desired pose relative to the current one (given in the global coordinate system)
00016 */
00017 
00018 #include <stdlib.h>
00019 #include <string.h>
00020 
00021 #include <ros/ros.h>
00022 #include <nav_msgs/Odometry.h>
00023 #include <osg/Quat>
00024 #include <osg/Vec3d>
00025 #include <osg/Matrix>
00026 
00027 bool firstpass=false;
00028 osg::Quat initialQ, goalQ, currentQ;
00029 osg::Vec3d initialT, goalT, currentT;
00030 double totalDistance, currentDistance;
00031 double x, y, z, roll, pitch, yaw;
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                 osg::Matrixd transform;
00038                 transform.setTrans(initialT);
00039                 transform.setRotate(initialQ);
00040 
00041                 osg::Matrixd T, Rx, Ry, Rz, transform_rel;
00042                 T.makeTranslate(x,y,z);
00043                 Rx.makeRotate(roll,1,0,0);
00044                 Ry.makeRotate(pitch,0,1,0);
00045                 Rz.makeRotate(yaw,0,0,1);
00046                 transform_rel=Rz*Ry*Rx*T;
00047                 transform=transform_rel*transform;
00048                 goalQ=transform.getRotate();
00049                 goalT=transform.getTrans();
00050 
00051                 totalDistance=(goalT-initialT).length();
00052                 firstpass=true;
00053         } 
00054         currentT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
00055         currentDistance=(goalT-currentT).length();
00056 }
00057 
00058 int main(int argc, char **argv) {
00059 
00060         ros::init(argc, argv, "gotoRelativePose");
00061         ros::NodeHandle nh;
00062 
00063         if (argc!=9) {
00064                 std::cerr << "USAGE: " << argv[0] << " <vehiclePoseTopic> <vehicleControlTopic> <rx> <ry> <rz> <rroll> <rpitch> <ryaw>" << std::endl;
00065                 std::cerr << "units are meters and radians." << std::endl;
00066                 return 0;
00067         }       
00068 
00069         std::string poseTopic(argv[1]);
00070         std::string controlTopic(argv[2]);
00071         x=atof(argv[3]);
00072         y=atof(argv[4]);
00073         z=atof(argv[5]);
00074         roll=atof(argv[6]);
00075         pitch=atof(argv[7]);
00076         yaw=atof(argv[8]);
00077 
00078         ros::Publisher position_pub=nh.advertise<nav_msgs::Odometry>(controlTopic,1);
00079         ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback);
00080 
00081         ros::Rate r(30);
00082         while (ros::ok()) {
00083            if (firstpass) {
00084                 osg::Vec3d vT=(goalT-currentT)*0.15;
00085                 double vScale=(vT.length()>0.05) ? 0.05/vT.length() : 1;
00086                 currentQ.slerp(1-currentDistance/totalDistance,initialQ, goalQ);
00087                 nav_msgs::Odometry odom;
00088                 odom.pose.pose.position.x=currentT.x()+vT.x()*vScale;
00089                 odom.pose.pose.position.y=currentT.y()+vT.y()*vScale;
00090                 odom.pose.pose.position.z=currentT.z()+vT.z()*vScale;
00091                 odom.pose.pose.orientation.x=currentQ.x();
00092                 odom.pose.pose.orientation.y=currentQ.y();
00093                 odom.pose.pose.orientation.z=currentQ.z();
00094                 odom.pose.pose.orientation.w=currentQ.w();
00095 
00096                 odom.twist.twist.linear.x=0;
00097                 odom.twist.twist.linear.y=0;
00098                 odom.twist.twist.linear.z=0;
00099                 odom.twist.twist.angular.x=0;
00100                 odom.twist.twist.angular.y=0;
00101                 odom.twist.twist.angular.z=0;
00102                 for (int i=0; i<36; i++) {
00103                         odom.twist.covariance[i]=0;
00104                         odom.pose.covariance[i]=0;
00105                 }
00106                 position_pub.publish(odom);
00107            }
00108            ros::spinOnce();
00109            r.sleep();
00110         }
00111 
00112         return 0;
00113 }


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