makeVehicleSurvey.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 
00020 #include <osg/Quat>
00021 #include <osg/Vec3d>
00022 
00023 int main(int argc, char **argv) {
00024 
00025 
00026         if (argc!=8) {
00027                 std::cerr << "USAGE: " << argv[0] << " <topic> <x> <y> <z> <yaw> <long> <step>" << std::endl;
00028                 std::cerr << "units in meters and radians" << std::endl;
00029                 return 0;
00030         }       
00031 
00032         std::string topic(argv[1]);
00033         double x=atof(argv[2]);
00034         double y=atof(argv[3]);
00035         double z=atof(argv[4]);
00036         // TODO FIXME yaw is not used below!
00037         //double yaw=atof(argv[5]);
00038         double amp=atof(argv[6]);
00039         double step=atof(argv[7]);
00040 
00041         std::string nodeName=topic;     
00042         nodeName=nodeName.replace(0,1,"a");
00043         std::cerr << "NodeName: " << nodeName << std::endl;
00044         ros::init(argc, argv, nodeName);
00045         ros::NodeHandle nh;
00046 
00047         ros::Publisher position_pub;
00048         position_pub=nh.advertise<nav_msgs::Odometry>(topic,1);
00049 
00050         double angle=0;
00051         ros::Rate r(20);
00052         while (ros::ok()) {
00053                 nav_msgs::Odometry odom;
00054                 odom.pose.pose.position.x=x+amp*sin(angle);
00055                 odom.pose.pose.position.y=y+angle*step;
00056                 odom.pose.pose.position.z=z;
00057 
00058                 double rz=0;
00059                 if (step>0) {
00060                         rz=M_PI_2-M_PI_2*cos(angle);
00061                 } else {
00062                         rz=-M_PI_2+M_PI_2*cos(angle);
00063                 }
00064                 osg::Quat rot(0, osg::Vec3d(1,0,0), 0, osg::Vec3d(0,1,0), rz, osg::Vec3d(0,0,1));
00065                 odom.pose.pose.orientation.x=rot.x();
00066                 odom.pose.pose.orientation.y=rot.y();
00067                 odom.pose.pose.orientation.z=rot.z();
00068                 odom.pose.pose.orientation.w=rot.w();
00069 
00070                 odom.twist.twist.linear.x=0;
00071                 odom.twist.twist.linear.y=0;
00072                 odom.twist.twist.linear.z=0;
00073                 odom.twist.twist.angular.x=0;
00074                 odom.twist.twist.angular.y=0;
00075                 odom.twist.twist.angular.z=0;
00076                 for (int i=0; i<36; i++) {
00077                         odom.twist.covariance[i]=0;
00078                         odom.pose.covariance[i]=0;
00079                 }
00080                 position_pub.publish(odom);
00081 
00082                 ros::spinOnce();
00083                 r.sleep();
00084                 angle+=0.01;
00085         }
00086 
00087         return 0;
00088 }


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