mocap_config.cpp
Go to the documentation of this file.
00001 /*
00002  *      _____
00003  *     /  _  \
00004  *    / _/ \  \
00005  *   / / \_/   \
00006  *  /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00007  *  \  / \_/ \  / /  _\| |  | __| / _ \ | ++ \ | ++ \ / _ \ |_   _|| | | |
00008  *   \ \_/ \_/ /  | |  | |  | ++ | |_| || ++ / | ++_/| |_| |  | |  | +-+ |
00009  *    \  \_/  /   | |_ | |_ | ++ |  _  || |\ \ | |   |  _  |  | |  | +-+ |
00010  *     \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00011  *             ROBOTICS™ 
00012  *
00013  *  File: mocap_config.cpp
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 #include <geometry_msgs/Pose.h>
00047 #include <geometry_msgs/Pose2D.h>
00048 #include <tf/transform_datatypes.h>
00049 #include "mocap_optitrack/mocap_config.h"
00050 
00051 const std::string POSE_TOPIC_PARAM_NAME = "pose";
00052 const std::string POSE2D_TOPIC_PARAM_NAME = "pose2d";
00053 const std::string FRAME_ID_PARAM_NAME = "frame_id";
00054 
00055 PublishedRigidBody::PublishedRigidBody(XmlRpc::XmlRpcValue &config_node)
00056 {
00057   // load configuration for this rigid body from ROS
00058   publish_pose = validateParam(config_node, POSE_TOPIC_PARAM_NAME);
00059   publish_pose2d = validateParam(config_node, POSE2D_TOPIC_PARAM_NAME);
00060   // only publish tf if a frame ID is provided
00061   publish_tf = validateParam(config_node, FRAME_ID_PARAM_NAME);
00062 
00063   if (publish_pose)
00064   {
00065     pose_topic = (std::string&) config_node[POSE_TOPIC_PARAM_NAME];
00066     pose_pub = n.advertise<geometry_msgs::Pose>(pose_topic, 1000);
00067   }
00068 
00069   if (publish_pose2d)
00070   {
00071     pose2d_topic = (std::string&) config_node[POSE2D_TOPIC_PARAM_NAME];
00072     pose2d_pub = n.advertise<geometry_msgs::Pose2D>(pose2d_topic, 1000);
00073   }
00074 
00075   if (publish_tf)
00076   {
00077     frame_id = (std::string&) config_node[FRAME_ID_PARAM_NAME];
00078   }
00079 }
00080 
00081 void PublishedRigidBody::publish(RigidBody &body)
00082 {
00083   // don't do anything if no new data was provided
00084   if (!body.has_data())
00085   {
00086     return;
00087   }
00088   // NaN?
00089   if (body.pose.position.x != body.pose.position.x)
00090   {
00091     return;
00092   }
00093 
00094   const geometry_msgs::Pose pose = body.get_ros_pose();
00095   
00096   if (publish_pose)
00097   {
00098     pose_pub.publish(pose);
00099   }
00100 
00101   if (!publish_pose2d && !publish_tf)
00102   {
00103     // nothing to do, bail early
00104     return;
00105   }
00106 
00107   tf::Quaternion q(pose.orientation.x,
00108                    pose.orientation.y,
00109                    pose.orientation.z,
00110                    pose.orientation.w);
00111 
00112   // publish 2D pose
00113   if (publish_pose2d)
00114   {
00115     geometry_msgs::Pose2D pose2d;
00116     pose2d.x = pose.position.x;
00117     pose2d.y = pose.position.y;
00118     pose2d.theta = tf::getYaw(q);
00119     pose2d_pub.publish(pose2d);
00120   }
00121 
00122   if (publish_tf)
00123   {
00124     // publish transform
00125     tf::Transform transform;
00126     transform.setOrigin( tf::Vector3(pose.position.x,
00127                                      pose.position.y,
00128                                      pose.position.z));
00129 
00130     // Handle different coordinate systems (Arena vs. rviz)
00131     transform.setRotation(q);
00132     ros::Time timestamp(ros::Time::now());
00133     tf_pub.sendTransform(tf::StampedTransform(transform, timestamp, "/odom", frame_id));
00134   }
00135 }
00136 
00137 bool PublishedRigidBody::validateParam(XmlRpc::XmlRpcValue &config_node, const std::string &name)
00138 {
00139   if (!config_node.hasMember(name))
00140   {
00141     return false;
00142   }
00143 
00144   if (config_node[name].getType() != XmlRpc::XmlRpcValue::TypeString)
00145   {
00146     return false;
00147   }
00148 
00149   return true;
00150 }
00151 


mocap_optitrack
Author(s): Kathrin Gräve/graeve@ais.uni-bonn.de, Alex Bencz/abencz@clearpathrobotics.com
autogenerated on Mon Oct 6 2014 02:22:13