pose_helper.cpp
Go to the documentation of this file.
1 
18 #include "helper/pose_helper.hpp"
19 
20 namespace directSearchWS {
21 
27 
29  if (!instancePtr) {
31  resetInstance();
32  }
33  return instancePtr;
34 }
35 
37  if (instancePtr) {
39  n.getParam("distanceFunc", distanceFuncParam);
40  ROS_INFO_STREAM("Param: distanceFunc: " << distanceFuncParam << " (1: GetDistance from robot_model_services, 2: euclidean distance)");
41 
42  double ncp = waitForParam<double>(n, "/nbv/ncp");
43  double fcp = waitForParam<double>(n, "/nbv/fcp");
44  viewPointDistance = (ncp + fcp) / 2.0;
45  ROS_INFO_STREAM("Param: viewPointDistance: " << viewPointDistance << " (== (/nbv/ncp + /nbv/fcp) / 2 = (" << ncp << "+" << fcp << ")/2");
46 
47  n.getParam("viewCenterPositionDistanceThreshold", viewCenterPositionDistanceThreshold);
48  ROS_INFO_STREAM("Param: viewCenterPositionDistanceThreshold: " << viewCenterPositionDistanceThreshold);
49 
50  viewCenterOrientationRadDistanceThreshold = waitForParam<double>(n, "/nbv/mHypothesisUpdaterAngleThreshold");
52  ROS_INFO_STREAM("Param: viewCenterOrientationRadDistanceThreshold (/nbv/mHypothesisUpdaterAngleThreshold): " << viewCenterOrientationRadDistanceThreshold);
53  }
54 }
55 
56 template <typename T>
57 T PoseHelper::waitForParam(ros::NodeHandle n, std::string param) {
58  if (!n.hasParam(param)) {
59  ROS_INFO_STREAM("Wait for param: " << param);
60  while (!n.hasParam(param)) {
61  ros::Duration(0.1).sleep();
62  }
63  }
64  T value;
65  n.getParam(param, value);
66  return value;
67 }
68 
71  getDistanceServiceClient = n.serviceClient<asr_robot_model_services::GetDistance>("/asr_robot_model_services/GetDistance");
72  getRobotPoseServiceClient = n.serviceClient<asr_robot_model_services::GetPose>("/asr_robot_model_services/GetRobotPose");
73  getCameraPoseServiceClient = n.serviceClient<asr_robot_model_services::GetPose>("/asr_robot_model_services/GetCameraPose");
74 }
75 
76 
77 double PoseHelper::calcDistPositionWithNBV(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
78  asr_robot_model_services::GetDistance srv;
79  srv.request.sourcePosition.x = pose1.position.x;
80  srv.request.sourcePosition.y = pose1.position.y;
81  srv.request.sourcePosition.z = pose1.position.z;
82  srv.request.targetPosition.x = pose2.position.x;
83  srv.request.targetPosition.y = pose2.position.y;
84  srv.request.targetPosition.z = pose2.position.z;
85  if (getDistanceServiceClient.call(srv)) {
86  return srv.response.distance;
87  } else {
88  ROS_ERROR("Failed to call service GetDistance, using euclian Distance instead");
89  return calcDistPositionEucl(pose1, pose2);
90  }
91 }
92 
93 double PoseHelper::calcDistPositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
94  return sqrt(pow(pose1.position.x - pose2.position.x, 2)
95  + pow(pose1.position.y - pose2.position.y, 2)
96  + pow(pose1.position.z - pose2.position.z, 2));
97 }
98 
99 double PoseHelper::calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
100  Eigen::Quaterniond rotation1 = convertPoseQuatToQuat(pose1);
101  rotation1.normalize();
102  Eigen::Quaterniond rotation2 = convertPoseQuatToQuat(pose2);
103  rotation2.normalize();
104  return rotation1.angularDistance(rotation2);
105 }
106 
107 Eigen::Vector3d PoseHelper::calculateViewCenterPoint(const geometry_msgs::Pose &pose) {
108  Eigen::Quaterniond targetOrientation = convertPoseQuatToQuat(pose);
109  Eigen::Vector3d targetViewVector = targetOrientation * Eigen::Vector3d::UnitX();
110  targetViewVector.normalize();
111  Eigen::Vector3d target_view_center_point = convertPositionToVec(pose) + targetViewVector * viewPointDistance;
112  return target_view_center_point;
113 }
114 
115 geometry_msgs::Pose PoseHelper::getCurrentRobotPose() {
116  asr_robot_model_services::GetPose srv;
117  if (getRobotPoseServiceClient.call(srv)) {
118  ROS_DEBUG_STREAM("CurrentRobotPose:\n" << srv.response.pose);
119  return srv.response.pose;
120  } else {
121  ROS_ERROR("Failed to call service GetRobotPose, using origin instead");
122  return getOriginPose();
123  }
124 }
125 
126 geometry_msgs::Pose PoseHelper::getCurrentCameraPose() {
127  asr_robot_model_services::GetPose srv;
128  if (getCameraPoseServiceClient.call(srv)) {
129  ROS_DEBUG_STREAM("CurrentCameraPose:\n" << srv.response.pose);
130  return srv.response.pose;
131  } else {
132  ROS_ERROR("Failed to call service GetCameraPose, using origin instead");
133  return getOriginPose();
134  }
135 }
136 
137 double PoseHelper::calculateDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
138  if (distanceFuncParam == 1) {
139  return calcDistPositionWithNBV(pose1, pose2);
140  } else {
141  return calcDistPositionEucl(pose1, pose2);
142  }
143 }
144 
145 bool PoseHelper::checkViewCenterPointsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2) {
146  Eigen::Vector3d viewCenterPoint1 = calculateViewCenterPoint(pose1);
147  Eigen::Vector3d viewCenterPoint2 = calculateViewCenterPoint(pose2);
148 
151 }
152 
153 bool PoseHelper::checkPosesAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2,
154  const double positionThreshold, const double orientationRadThreshold) {
155  return checkPositionsAreApproxEquale(pose1, pose2, positionThreshold)
156  && checkOrientationsAreApproxEquale(pose1, pose2, orientationRadThreshold);
157 }
158 
159 bool PoseHelper::checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold) {
160  const double distance_position = calcDistPositionEucl(pose1, pose2);
161  return positionThreshold > distance_position;
162 }
163 
164 bool PoseHelper::checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientationRadThreshold) {
165  const double distance_orientation_rad = calcAngularDistanceInRad(pose1, pose2);
166  return orientationRadThreshold > distance_orientation_rad;
167 }
168 
169 }
170 
171 
172 
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
geometry_msgs::Pose getCurrentRobotPose()
bool checkViewCenterPointsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
static double viewCenterPositionDistanceThreshold
Definition: pose_helper.hpp:40
geometry_msgs::Pose convertVecToPosition(const Eigen::Vector3d &vec)
Definition: pose_helper.hpp:61
double calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: pose_helper.cpp:99
double calcDistPositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: pose_helper.cpp:93
bool sleep() const
bool call(MReq &req, MRes &res)
static T waitForParam(ros::NodeHandle n, std::string param)
Definition: pose_helper.cpp:57
ROSCPP_DECL const std::string & getName()
#define DEG_TO_RAD
Definition: pose_helper.hpp:30
double calcDistPositionWithNBV(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: pose_helper.cpp:77
geometry_msgs::Pose getOriginPose()
Definition: pose_helper.hpp:73
static double viewPointDistance
Definition: pose_helper.hpp:38
boost::shared_ptr< PoseHelper > PoseHelperPtr
Eigen::Vector3d calculateViewCenterPoint(const geometry_msgs::Pose &pose)
geometry_msgs::Pose getCurrentCameraPose()
ros::ServiceClient getDistanceServiceClient
Definition: pose_helper.hpp:43
bool checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold)
static boost::shared_ptr< PoseHelper > getInstance()
Definition: pose_helper.cpp:28
bool checkPosesAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold, const double orientationRadThreshold)
#define ROS_DEBUG_STREAM(args)
Eigen::Quaterniond convertPoseQuatToQuat(const geometry_msgs::Pose &pose)
Definition: pose_helper.hpp:69
ros::ServiceClient getCameraPoseServiceClient
Definition: pose_helper.hpp:45
#define ROS_INFO_STREAM(args)
bool getParam(const std::string &key, std::string &s) const
bool checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientationRadThreshold)
Eigen::Vector3d convertPositionToVec(const geometry_msgs::Pose &pose)
Definition: pose_helper.hpp:57
bool hasParam(const std::string &key) const
double calculateDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
ros::ServiceClient getRobotPoseServiceClient
Definition: pose_helper.hpp:44
#define ROS_ERROR(...)
static double viewCenterOrientationRadDistanceThreshold
Definition: pose_helper.hpp:41
static boost::shared_ptr< PoseHelper > instancePtr
Definition: pose_helper.hpp:37


asr_direct_search_manager
Author(s): Borella Jocelyn, Karrenbauer Oliver, Mei├čner Pascal
autogenerated on Wed Jan 8 2020 03:15:41