Go to the documentation of this file.00001 #include "transform_pose_alg_node.h"
00002
00003 TransformPoseAlgNode::TransformPoseAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<TransformPoseAlgorithm>()
00005 {
00006
00007
00008
00009
00010
00011
00012
00013
00014 this->transform_pose_server_ = this->public_node_handle_.advertiseService("transform_pose", &TransformPoseAlgNode::transform_poseCallback, this);
00015
00016
00017
00018
00019
00020
00021 }
00022
00023 TransformPoseAlgNode::~TransformPoseAlgNode(void)
00024 {
00025
00026 }
00027
00028 void TransformPoseAlgNode::mainNodeThread(void)
00029 {
00030
00031
00032
00033
00034
00035
00036
00037 }
00038
00039
00040
00041
00042 bool TransformPoseAlgNode::transform_poseCallback(estirabot_msgs::TransformPose::Request &req, estirabot_msgs::TransformPose::Response &res)
00043 {
00044 ROS_DEBUG("TransformPoseAlgNode::transform_poseCallback: New Request Received!");
00045
00046
00047
00048
00049
00050
00051
00052 geometry_msgs::PoseStamped pose_wam;
00053 ros::Time common_time;
00054 std::string error_aux;
00055
00056 tf_listener.getLatestCommonTime(req.target_frame, req.orig_pose_st.header.frame_id, common_time, &error_aux);
00057 tf_listener.transformPose(req.target_frame, common_time, req.orig_pose_st, req.orig_pose_st.header.frame_id, res.target_pose_st);
00058
00059
00060
00061
00062
00063 return true;
00064 }
00065
00066
00067
00068
00069
00070 void TransformPoseAlgNode::node_config_update(Config &config, uint32_t level)
00071 {
00072 this->alg_.lock();
00073
00074 this->alg_.unlock();
00075 }
00076
00077 void TransformPoseAlgNode::addNodeDiagnostics(void)
00078 {
00079 }
00080
00081
00082 int main(int argc,char *argv[])
00083 {
00084 return algorithm_base::main<TransformPoseAlgNode>(argc, argv, "transform_pose_alg_node");
00085 }