00001 /* 00002 * _____ 00003 * / _ \ 00004 * / _/ \ \ 00005 * / / \_/ \ 00006 * / \_/ _ \ ___ _ ___ ___ ____ ____ ___ _____ _ _ 00007 * \ / \_/ \ / / _\| | | __| / _ \ | ++ \ | ++ \ / _ \ |_ _|| | | | 00008 * \ \_/ \_/ / | | | | | ++ | |_| || ++ / | ++_/| |_| | | | | +-+ | 00009 * \ \_/ / | |_ | |_ | ++ | _ || |\ \ | | | _ | | | | +-+ | 00010 * \_____/ \___/|___||___||_| |_||_| \_\|_| |_| |_| |_| |_| |_| 00011 * ROBOTICS™ 00012 * 00013 * File: mocap_config.h 00014 * Desc: Classes representing ROS configuration for mocap_optitrack node. Data 00015 * will be published to differed topics based on the configuration provided. 00016 * Auth: Alex Bencz 00017 * 00018 * Copyright (c) 2012, Clearpath Robotics, Inc. 00019 * All Rights Reserved 00020 * 00021 * Redistribution and use in source and binary forms, with or without 00022 * modification, are permitted provided that the following conditions are met: 00023 * * Redistributions of source code must retain the above copyright 00024 * notice, this list of conditions and the following disclaimer. 00025 * * Redistributions in binary form must reproduce the above copyright 00026 * notice, this list of conditions and the following disclaimer in the 00027 * documentation and/or other materials provided with the distribution. 00028 * * Neither the name of Clearpath Robotics, Inc. nor the 00029 * names of its contributors may be used to endorse or promote products 00030 * derived from this software without specific prior written permission. 00031 * 00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00033 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00034 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00035 * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY 00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00038 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00039 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00040 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00041 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00042 * 00043 * Please send comments, questions, or patches to skynet@clearpathrobotics.com 00044 * 00045 */ 00046 00047 #ifndef __MOCAP_CONFIG_H__ 00048 #define __MOCAP_CONFIG_H__ 00049 00050 #include <ros/ros.h> 00051 #include <tf/transform_broadcaster.h> 00052 #include "mocap_datapackets.h" 00053 00054 class PublishedRigidBody 00055 { 00056 private: 00057 ros::NodeHandle n; 00058 00059 std::string pose_topic; 00060 std::string pose2d_topic; 00061 std::string frame_id; 00062 00063 bool publish_pose; 00064 bool publish_tf; 00065 bool publish_pose2d; 00066 00067 tf::TransformBroadcaster tf_pub; 00068 ros::Publisher pose_pub; 00069 ros::Publisher pose2d_pub; 00070 00071 bool validateParam(XmlRpc::XmlRpcValue &, const std::string &); 00072 00073 public: 00074 PublishedRigidBody(XmlRpc::XmlRpcValue &); 00075 void publish(RigidBody &); 00076 }; 00077 00078 typedef std::map<int, PublishedRigidBody> RigidBodyMap; 00079 typedef std::pair<int, PublishedRigidBody> RigidBodyItem; 00080 00081 #endif // __MOCAP_CONFIG_H__