00001 #ifndef ROBOT_POSE_VISUALIZATION_H 00002 #define ROBOT_POSE_VISUALIZATION_H 00003 00004 #include <ros/ros.h> 00005 #include <arm_navigation_msgs/RobotState.h> 00006 #include <visualization_msgs/MarkerArray.h> 00007 #include <geometry_msgs/PoseStamped.h> 00008 00010 00020 class RobotPoseVisualization 00021 { 00022 public: 00023 RobotPoseVisualization(); 00024 ~RobotPoseVisualization(); 00025 00027 bool initialize(); 00028 00030 void resetRobotState(); 00031 00033 void updateRobotStateJoints(const sensor_msgs::JointState & js); 00034 00036 void updateRobotStatePose(const geometry_msgs::PoseStamped & pose); 00037 00039 visualization_msgs::MarkerArray getMarkers(const std_msgs::ColorRGBA & color, const std::string & ns); 00040 00041 private: 00042 bool currentStateInitialized() const; 00043 00044 private: 00045 arm_navigation_msgs::RobotState _initState; 00046 arm_navigation_msgs::RobotState _currentState; 00047 00048 ros::ServiceClient _srvGetRobotMarker; 00049 }; 00050 00051 #endif 00052