setVehicleVelocity.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 
00021 int main(int argc, char **argv) {
00022 
00023 
00024         if (argc!=8) {
00025                 std::cerr << "USAGE: " << argv[0] << " <topic> <vx> <vy> <vz> <vroll> <vpitch> <vyaw>" << std::endl;
00026                 std::cerr << "units are displacement/simulated_time. Time scale to be implemented." << std::endl;
00027                 return 0;
00028         }       
00029 
00030         std::string topic(argv[1]);
00031         double x=atof(argv[2]);
00032         double y=atof(argv[3]);
00033         double z=atof(argv[4]);
00034         double roll=atof(argv[5]);
00035         double pitch=atof(argv[6]);
00036         double yaw=atof(argv[7]);
00037 
00038         //std::string nodeName=topic;
00039         //nodeName.replace(0,1,"a");
00040         ros::init(argc, argv, "setVehicleVelocity");
00041         ros::NodeHandle nh;
00042         ros::Publisher position_pub;
00043         position_pub=nh.advertise<nav_msgs::Odometry>(topic,1);
00044 
00045         ros::Rate r(25);
00046         while (ros::ok()) {
00047                 nav_msgs::Odometry odom;
00048                 odom.pose.pose.position.x=0.0;
00049                 odom.pose.pose.position.y=0.0;
00050                 odom.pose.pose.position.z=0.0;
00051                 odom.pose.pose.orientation.x=0.0;
00052                 odom.pose.pose.orientation.y=0.0;
00053                 odom.pose.pose.orientation.z=0.0;
00054                 odom.pose.pose.orientation.w=1;
00055 
00056                 odom.twist.twist.linear.x=x;
00057                 odom.twist.twist.linear.y=y;
00058                 odom.twist.twist.linear.z=z;
00059                 odom.twist.twist.angular.x=roll;
00060                 odom.twist.twist.angular.y=pitch;
00061                 odom.twist.twist.angular.z=yaw;
00062                 for (int i=0; i<36; i++) {
00063                         odom.twist.covariance[i]=0;
00064                         odom.pose.covariance[i]=0;
00065                 }
00066                 position_pub.publish(odom);
00067 
00068                 ros::spinOnce();
00069                 r.sleep();
00070         }
00071 
00072         return 0;
00073 }


uwsim
Author(s): Mario Prats
autogenerated on Mon Oct 6 2014 08:24:07