00001 /****************************************************************************** 00002 Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 00003 00004 Redistribution and use in source and binary forms, with or without 00005 modification, are permitted provided that the following conditions are met: 00006 00007 * Redistributions of source code must retain the above copyright notice, this 00008 list of conditions and the following disclaimer. 00009 00010 * Redistributions in binary form must reproduce the above copyright notice, 00011 this list of conditions and the following disclaimer in the documentation 00012 and/or other materials provided with the distribution. 00013 00014 * Neither the name of the copyright holder nor the names of its 00015 contributors may be used to endorse or promote products derived from 00016 this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00028 ******************************************************************************/ 00029 00030 #include <ros/ros.h> 00031 00032 #include <xpp_vis/urdf_visualizer.h> 00033 00034 #include <xpp_msgs/RobotStateCartesian.h> 00035 #include <xpp_msgs/topic_names.h> 00036 00037 using namespace xpp; 00038 00039 ros::Publisher joint_state_pub; 00040 00041 // convert cartesian to joint state message (just by extracting base) 00042 void StateCallback(const xpp_msgs::RobotStateCartesian& msg) 00043 { 00044 xpp_msgs::RobotStateJoint joint_msg; 00045 joint_msg.base = msg.base; 00046 joint_state_pub.publish(joint_msg); 00047 } 00048 00049 00050 int main(int argc, char *argv[]) 00051 { 00052 ::ros::init(argc, argv, "quadrotor_urdf_visualizer"); 00053 00054 ::ros::NodeHandle n; 00055 const std::string joint_quadrotor = "xpp/joint_quadrotor_des"; 00056 ros::Subscriber cart_state_sub = n.subscribe(xpp_msgs::robot_state_desired, 1, StateCallback); 00057 joint_state_pub = n.advertise<xpp_msgs::RobotStateJoint>(joint_quadrotor, 1); 00058 00059 // publish base state to RVIZ 00060 std::string urdf = "quadrotor_rviz_urdf_robot_description"; 00061 UrdfVisualizer node_des(urdf, {}, "base", "world", joint_quadrotor, "quadrotor"); 00062 00063 ::ros::spin(); 00064 00065 return 0; 00066 } 00067