00001 #ifndef CONVENIENCE_ROS_FUNCTIONS_ROBOTINFO 00002 #define CONVENIENCE_ROS_FUNCTIONS_ROBOTINFO 00003 00004 #include <ros/ros.h> 00005 #include <baselib_binding/Thread.h> 00006 #include <geometry_msgs/PoseStamped.h> 00007 #include <sensor_msgs/JointState.h> 00008 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00009 #include <sensor_msgs/MultiDOFJointState.h> 00010 #include <convenience_ros_functions/ROSFunctions.h> 00011 00012 namespace convenience_ros_functions { 00013 00022 class RobotInfo { 00023 00024 public: 00031 RobotInfo(const std::string& default_robot_pose_topic="/amcl_pose", 00032 const std::string& default_joint_states_topic="/joint_states"); 00033 00034 ~RobotInfo(); 00035 00036 geometry_msgs::PoseStamped getCurrentRobotPose(ros::NodeHandle& n); 00037 geometry_msgs::PoseStamped getCurrentRobotPose(const std::string& topicName, ros::NodeHandle& n); 00038 00039 geometry_msgs::PoseWithCovarianceStamped getCurrentRobotPoseWithCovariance(ros::NodeHandle& n); 00040 geometry_msgs::PoseWithCovarianceStamped getCurrentRobotPoseWithCovariance(const std::string& topicName, ros::NodeHandle& n); 00041 00042 sensor_msgs::JointState getCurrentJointState(ros::NodeHandle& n); 00043 sensor_msgs::JointState getCurrentJointState(const std::string& topicName, ros::NodeHandle& n); 00044 00053 static sensor_msgs::MultiDOFJointState getVirtualJointState(const geometry_msgs::PoseStamped& robotPose, 00054 const std::string& virtualJointName, const std::string& frameID); 00055 00059 sensor_msgs::MultiDOFJointState getCurrentVirtualJointState(const std::string& poseTopicName, 00060 ros::NodeHandle& n, const std::string& virtualJointName, const std::string& frameID); 00061 00062 private: 00063 typedef baselib_binding::mutex mutex; 00064 typedef baselib_binding::unique_lock<mutex>::type unique_lock; 00065 00066 bool receivedJointState(); 00067 void jointStateCallback(const sensor_msgs::JointState& jointstate); 00068 bool receivedRobotPose(); 00069 void poseCallback(const geometry_msgs::PoseWithCovarianceStamped& msg); 00070 00071 mutex poseMutex; 00072 bool received_rp; 00073 geometry_msgs::PoseWithCovarianceStamped pose; 00074 00075 mutex jointStateMutex; 00076 sensor_msgs::JointState jointState; 00077 bool received_js; 00078 00079 std::string robot_pose_topic; 00080 std::string joint_states_topic; 00081 }; 00082 00083 } 00084 #endif // CONVENIENCE_ROS_FUNCTIONS_ROBOTINFO