setVehicleDisturbances.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 #include <stdlib.h>
00014 #include <string.h>
00015 
00016 //ROS
00017 #include <ros/ros.h>
00018 #include <nav_msgs/Odometry.h>
00019 //OSG
00020 #include <osg/Quat>
00021 #include <osg/Vec3d>
00022 #include <osg/Matrix>
00023 
00024 bool firstpass=false;
00025 osg::Quat initialQ;
00026 osg::Vec3d initialT;
00027 osg::Matrix wMv_initial;
00028 
00029 void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
00030         if (!firstpass) {
00031                 initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
00032                 initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
00033                 wMv_initial.setTrans(initialT);
00034                 wMv_initial.setRotate(initialQ);
00035                 firstpass=true;
00036         } 
00037 }
00038 
00039 int main(int argc, char **argv) {
00040 
00041         if (argc!=9) {
00042                 std::cerr << "USAGE: " << argv[0] << " <vehiclePoseTopic> <controlTopic> <ax> <ay> <az> <aroll> <apitch> <ayaw>" << std::endl;
00043                 std::cerr << "<ax> <ay> <az> <ayaw> is the maximum desired variation on the vehicle pose" << std::endl;
00044                 std::cerr << "units in meters and radians" << std::endl;
00045                 std::cerr << "Example: " << argv[0] << "/dataNavigator 0.2 0.2 0.2 0.2" << std::endl;
00046                 return 0;
00047         }       
00048 
00049         std::string poseTopic(argv[1]);
00050         std::string controlTopic(argv[2]);
00051         double ax=atof(argv[3]);
00052         double ay=atof(argv[4]);
00053         double az=atof(argv[5]);
00054         double aroll=atof(argv[6]);
00055         double apitch=atof(argv[7]);
00056         double ayaw=atof(argv[8]);
00057 
00058         std::string nodeName=controlTopic;
00059         nodeName.replace(0,1,"a");
00060         ros::init(argc, argv, nodeName);
00061         ros::NodeHandle nh;
00062         ros::Publisher position_pub;
00063         position_pub=nh.advertise<nav_msgs::Odometry>(controlTopic,1);
00064         ros::Subscriber position_sub = nh.subscribe(poseTopic, 1, vehiclePoseCallback);
00065 
00066         double angle=0;
00067         ros::Rate r(25);
00068         while (ros::ok()) {
00069            if (firstpass) {
00070                 osg::Matrixd T, Rx, Ry, Rz, transform;
00071                 T.makeTranslate(ax*sin(2*angle),ay*sin(angle),az*sin(angle));
00072                 Rx.makeRotate(aroll*sin(angle),1,0,0);
00073                 Ry.makeRotate(apitch*sin(angle),0,1,0);
00074                 Rz.makeRotate(ayaw*sin(angle),0,0,1);
00075                 transform=Rz*Ry*Rx*T*wMv_initial;
00076                 
00077                 osg::Vec3d currentT=transform.getTrans();
00078                 osg::Quat currentQ=transform.getRotate();
00079 
00080                 nav_msgs::Odometry odom;
00081                 odom.pose.pose.position.x=currentT.x();
00082                 odom.pose.pose.position.y=currentT.y();
00083                 odom.pose.pose.position.z=currentT.z();
00084                 odom.pose.pose.orientation.x=currentQ.x();
00085                 odom.pose.pose.orientation.y=currentQ.y();
00086                 odom.pose.pose.orientation.z=currentQ.z();
00087                 odom.pose.pose.orientation.w=currentQ.w();
00088                 angle+=0.01;
00089 
00090                 odom.twist.twist.linear.x=0;
00091                 odom.twist.twist.linear.y=0;
00092                 odom.twist.twist.linear.z=0;
00093                 odom.twist.twist.angular.x=0;
00094                 odom.twist.twist.angular.y=0;
00095                 odom.twist.twist.angular.z=0;
00096                 for (int i=0; i<36; i++) {
00097                         odom.twist.covariance[i]=0;
00098                         odom.pose.covariance[i]=0;
00099                 }
00100                 position_pub.publish(odom);
00101            }
00102            ros::spinOnce();
00103            r.sleep();
00104 
00105         }
00106 
00107         return 0;
00108 }


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