Go to the documentation of this file.00001 #include "zyonz_camera_pose_around_point_alg_node.h"
00002
00003 ZyonzCameraPoseAroundPointAlgNode::ZyonzCameraPoseAroundPointAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<ZyonzCameraPoseAroundPointAlgorithm>()
00005 {
00006
00007
00008
00009
00010
00011
00012
00013
00014 get_camera_pose_server_ = public_node_handle_.advertiseService("get_camera_pose", &ZyonzCameraPoseAroundPointAlgNode::get_camera_poseCallback, this);
00015
00016
00017
00018
00019
00020
00021 }
00022
00023 ZyonzCameraPoseAroundPointAlgNode::~ZyonzCameraPoseAroundPointAlgNode(void)
00024 {
00025
00026 }
00027
00028 void ZyonzCameraPoseAroundPointAlgNode::mainNodeThread(void)
00029 {
00030
00031
00032
00033
00034
00035
00036
00037 }
00038
00039
00040
00041
00042 bool ZyonzCameraPoseAroundPointAlgNode::get_camera_poseCallback(zyonz_camera_pose_around_point::GetPose::Request &req, zyonz_camera_pose_around_point::GetPose::Response &res)
00043 {
00044 ROS_INFO("ZyonzCameraPoseAroundPointAlgNode::get_camera_poseCallback: New Request Received!");
00045
00046
00047 alg_.lock();
00048 get_camera_pose_mutex_.enter();
00049
00050 if(!alg_.isRunning(req.view_point, req.dist_m, req.rad_x_rot, req.rad_y_rot))
00051 {
00052 ROS_INFO("ZyonzCameraPoseAroundPointAlgNode::get_camera_poseCallback: ERROR: alg is not on run mode yet.");
00053 }
00054
00055 res.camera_pose = *alg_.get_camera_pose();
00056
00057
00058 alg_.unlock();
00059 get_camera_pose_mutex_.exit();
00060
00061 return true;
00062 }
00063
00064
00065
00066
00067
00068 void ZyonzCameraPoseAroundPointAlgNode::node_config_update(Config &config, uint32_t level)
00069 {
00070 this->alg_.lock();
00071
00072 this->alg_.unlock();
00073 }
00074
00075 void ZyonzCameraPoseAroundPointAlgNode::addNodeDiagnostics(void)
00076 {
00077 }
00078
00079
00080 int main(int argc,char *argv[])
00081 {
00082 return algorithm_base::main<ZyonzCameraPoseAroundPointAlgNode>(argc, argv, "zyonz_camera_pose_around_point_alg_node");
00083 }