pr2_plugs_common: pr2_plugs_common: detector

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Wim Meeussen
00036 *********************************************************************/
00037 
00038 
00039 #include <ros/ros.h>
00040 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00041 #include <kdl/frames.hpp>
00042 
00043 int main(int argc, char** argv)
00044 {
00045   ros::init(argc, argv, "detector_stub");
00046   double x, y, z, Rx, Ry, Rz;
00047   std::string frame_id;
00048   ros::NodeHandle node("~");
00049   node.param("x", x, 0.0);
00050   node.param("y", y, 0.0);
00051   node.param("z", z, 0.0);
00052   node.param("Rx", Rx, 0.0);
00053   node.param("Ry", Ry, 0.0);
00054   node.param("Rz", Rz, 0.0);
00055   node.param("frame_id", frame_id, std::string("frame_id_not_specified"));
00056 
00057   ros::NodeHandle node_top;
00058   ros::Publisher pub_pose = node_top.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 10);
00059   geometry_msgs::PoseWithCovarianceStamped pose;
00060   pose.header.frame_id = frame_id;
00061   pose.pose.pose.position.x = x;
00062   pose.pose.pose.position.y = y;
00063   pose.pose.pose.position.z = z;
00064   KDL::Rotation tmp = KDL::Rotation::RPY(Rx, Ry, Rz);
00065   double Qx, Qy, Qz, Qw;
00066   tmp.GetQuaternion(Qx, Qy, Qz, Qw);
00067   pose.pose.pose.orientation.x = Qx;
00068   pose.pose.pose.orientation.y = Qy;
00069   pose.pose.pose.orientation.z = Qz;
00070   pose.pose.pose.orientation.w = Qw;
00071 
00072   double cov[36] =  {0.0001, 0, 0, 0, 0, 0,
00073                      0, 0.0001, 0, 0, 0, 0,
00074                      0, 0, 0.0001, 0, 0, 0,
00075                      0, 0, 0, 0.0001, 0, 0,
00076                      0, 0, 0, 0, 0.0001, 0,
00077                      0, 0, 0, 0, 0, 0.0001};
00078   for (unsigned int i=0; i<36; i++)
00079     pose.pose.covariance[i] = cov[i];
00080 
00081   while (ros::ok()){
00082     pose.header.stamp = ros::Time::now();
00083     pub_pose.publish(pose);
00084     ros::Duration(0.4).sleep();
00085   }
00086   return 0;
00087 }
detector_stub.cpp
