setVehiclePose.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 <geometry_msgs/Pose.h>
00019 #include <osg/Quat>
00020 #include <osg/Vec3d>
00021 #include <osg/Matrix>
00022 
00023 
00024 int main(int argc, char **argv) {
00025 
00026         ros::init(argc, argv, "setVehiclePose");
00027         ros::NodeHandle nh;
00028 
00029         if (argc!=8) {
00030                 std::cerr << "USAGE: " << argv[0] << " <topic> <x> <y> <z> <roll> <pitch> <yaw>" << std::endl;
00031                 std::cerr << "units are in meters and radians." << std::endl;
00032                 return 0;
00033         }       
00034 
00035         std::string topic(argv[1]);
00036         double x=atof(argv[2]);
00037         double y=atof(argv[3]);
00038         double z=atof(argv[4]);
00039         double roll=atof(argv[5]);
00040         double pitch=atof(argv[6]);
00041         double yaw=atof(argv[7]);
00042 
00043         ros::Publisher position_pub;
00044         position_pub=nh.advertise<geometry_msgs::Pose>(topic,1);
00045 
00046 
00047         osg::Matrixd T, Rx, Ry, Rz, transform;
00048         T.makeTranslate(x,y,z);
00049         Rx.makeRotate(roll,1,0,0);
00050         Ry.makeRotate(pitch,0,1,0);
00051         Rz.makeRotate(yaw,0,0,1);
00052         transform=Rz*Ry*Rx*T;
00053         osg::Vec3d trans=transform.getTrans();
00054         osg::Quat rot=transform.getRotate();
00055 
00056 
00057         ros::Rate r(25);
00058         while (ros::ok()) {
00059                 geometry_msgs::Pose pose;
00060 
00061                 pose.position.x=trans.x();
00062                 pose.position.y=trans.y();
00063                 pose.position.z=trans.z();
00064                 pose.orientation.x=rot.x();
00065                 pose.orientation.y=rot.y();
00066                 pose.orientation.z=rot.z();
00067                 pose.orientation.w=rot.w();
00068                 
00069                 position_pub.publish(pose);
00070 
00071                 ros::spinOnce();
00072                 r.sleep();
00073         }
00074 
00075         return 0;
00076 }


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