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 #ifndef XPP_VIS_URDF_VISUALIZER_H_ 00031 #define XPP_VIS_URDF_VISUALIZER_H_ 00032 00033 #include <cstdlib> 00034 #include <iostream> 00035 #include <map> 00036 #include <memory> 00037 #include <string> 00038 #include <vector> 00039 00040 #include <ros/ros.h> 00041 #include <tf/transform_broadcaster.h> 00042 #include <ros/package.h> 00043 #include <urdf/model.h> 00044 #include <sensor_msgs/JointState.h> 00045 #include <robot_state_publisher/robot_state_publisher.h> 00046 #include <kdl_parser/kdl_parser.hpp> 00047 00048 #include <xpp_msgs/RobotStateJoint.h> 00049 #include <xpp_states/joints.h> 00050 00051 00052 namespace xpp { 00053 00060 class UrdfVisualizer { 00061 public: 00062 using URDFName = std::string; 00063 using UrdfnameToJointAngle = std::map<URDFName, double>; 00064 00078 UrdfVisualizer(const std::string& urdf_name, 00079 const std::vector<URDFName>& joint_names_in_urdf, 00080 const URDFName& base_link_in_urdf, 00081 const std::string& rviz_fixed_frame, 00082 const std::string& state_topic, 00083 const std::string& tf_prefix = ""); 00084 virtual ~UrdfVisualizer() = default; 00085 00086 private: 00087 ros::Subscriber state_sub_des_; 00088 tf::TransformBroadcaster tf_broadcaster_; 00089 std::shared_ptr<robot_state_publisher::RobotStatePublisher> robot_publisher; 00090 00091 void StateCallback(const xpp_msgs::RobotStateJoint& msg); 00092 00093 UrdfnameToJointAngle AssignAngleToURDFJointName(const sensor_msgs::JointState &msg) const; 00094 geometry_msgs::TransformStamped GetBaseFromRos(const ::ros::Time& stamp, 00095 const geometry_msgs::Pose &msg) const; 00096 00097 std::vector<URDFName> joint_names_in_urdf_; 00098 URDFName base_joint_in_urdf_; 00099 00100 std::string state_msg_name_; 00101 std::string rviz_fixed_frame_; 00102 std::string tf_prefix_; 00103 }; 00104 00105 } // namespace xpp 00106 00107 #endif /* XPP_VIS_URDF_VISUALIZER_H_ */