velocity_node.cpp
Go to the documentation of this file.00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 * 00029 */ 00030 00039 #include <semanticmodel/velocity.h> 00040 #include <ros/ros.h> 00041 00042 namespace sm=semanticmodel; 00043 namespace mr=mongo_ros; 00044 namespace gm=geometry_msgs; 00045 00046 using std::cerr; 00047 using boost::shared_ptr; 00048 00049 int main (int argc, char** argv) 00050 { 00051 // Parse args 00052 ros::init(argc, argv, "velocity_node"); 00053 if (argc!=7) 00054 { 00055 cerr << "Usage: " << argv[0] << 00056 " POINT_FRAME X Y Z REF_FRAME PUBLICATION_RATE(hz)\n"; 00057 return 1; 00058 } 00059 00060 // Initialize ros comms 00061 ros::NodeHandle nh("~"); 00062 shared_ptr<mr::LiveTransformSource> trans(new mr::LiveTransformSource); 00063 sm::VelocityCalc vel_calc(trans, argv[5]); 00064 ros::Publisher pub = nh.advertise<gm::Vector3>("velocity", 10); 00065 ros::Rate r(atof(argv[6])); 00066 00067 // Main loop 00068 while (ros::ok()) 00069 { 00070 r.sleep(); 00071 00072 // Form point 00073 gm::PointStamped p; 00074 p.header.frame_id = argv[1]; 00075 p.header.stamp = ros::Time::now(); 00076 p.point.x = atof(argv[2]); 00077 p.point.y = atof(argv[3]); 00078 p.point.z = atof(argv[4]); 00079 00080 // Find velocity and publish 00081 try 00082 { 00083 const gm::Vector3 vel = vel_calc.velocity(p); 00084 pub.publish(vel); 00085 } 00086 catch (tf::TransformException& e) 00087 { 00088 ROS_WARN ("Skipping velocity pub due to tf exception '%s'", 00089 e.what()); 00090 } 00091 } 00092 return 0; 00093 }