pose_helper.hpp
Go to the documentation of this file.
1 
18 #pragma once
19 
20 #include <ros/ros.h>
21 #include <math.h>
22 #include <boost/shared_ptr.hpp>
23 #include <eigen3/Eigen/Geometry>
24 
25 #include "asr_robot_model_services/GetDistance.h"
26 #include "asr_robot_model_services/GetPose.h"
27 #include "asr_robot_model_services/RobotStateMessage.h"
28 
29 #define RAD_TO_DEG 180.0 / M_PI
30 #define DEG_TO_RAD M_PI / 180.0
31 
32 namespace directSearchWS {
33 
34 class PoseHelper {
35 
36 private:
38  static double viewPointDistance;
39  static int distanceFuncParam;
42 
46 
47  template <typename T>
48  static T waitForParam(ros::NodeHandle n, std::string param);
49 
50  //calc distance functions
51  double calcDistPositionWithNBV(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
52  double calcDistPositionEucl(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
53 
54  Eigen::Vector3d calculateViewCenterPoint(const geometry_msgs::Pose &pose);
55 
56 
57  Eigen::Vector3d convertPositionToVec(const geometry_msgs::Pose &pose) {
58  return Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
59  }
60 
61  geometry_msgs::Pose convertVecToPosition(const Eigen::Vector3d &vec) {
62  geometry_msgs::Pose pose;
63  pose.position.x = vec[0];
64  pose.position.y = vec[1];
65  pose.position.z = vec[2];
66  return pose;
67  }
68 
69  Eigen::Quaterniond convertPoseQuatToQuat(const geometry_msgs::Pose &pose) {
70  return Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
71  }
72 
73  geometry_msgs::Pose getOriginPose() {
74  geometry_msgs::Pose origin;
75  origin.position.x = 0;
76  origin.position.y = 0;
77  origin.position.z = 0;
78  origin.orientation.x = 0;
79  origin.orientation.y = 0;
80  origin.orientation.z = 0;
81  origin.orientation.w = 1;
82  return origin;
83  }
84 
85 
86  PoseHelper();
87 
88 public:
89 
91  static void resetInstance();
92 
93  geometry_msgs::Pose getCurrentRobotPose();
94  geometry_msgs::Pose getCurrentCameraPose();
95 
96  double calculateDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
97  double calcAngularDistanceInRad(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
98 
99  bool checkViewCenterPointsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
100 
101  bool checkPosesAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold, const double orientationRadThreshold);
102  bool checkPositionsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double positionThreshold);
103  bool checkOrientationsAreApproxEquale(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2, const double orientationRadThreshold);
104 
105 };
106 
108 
109 }
110 
111 
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
static T waitForParam(ros::NodeHandle n, std::string param)
Definition: pose_helper.cpp:57
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)
Eigen::Quaterniond convertPoseQuatToQuat(const geometry_msgs::Pose &pose)
Definition: pose_helper.hpp:69
ros::ServiceClient getCameraPoseServiceClient
Definition: pose_helper.hpp:45
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
double calculateDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
ros::ServiceClient getRobotPoseServiceClient
Definition: pose_helper.hpp:44
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