IKTest.cpp
Go to the documentation of this file.
1 
20 #define BOOST_TEST_STATIC_LINK
21 
22 #include <boost/test/included/unit_test.hpp>
23 #include "robot_model_services/test_cases/BaseTest.h"
24 #include "robot_model_services/robot_model/impl/MILDRobotModelWithExactIK.hpp"
25 #include "robot_model_services/robot_model/impl/MILDRobotModel.hpp"
26 #include "robot_model_services/robot_model/impl/MILDRobotState.hpp"
27 #include "geometry_msgs/PoseWithCovarianceStamped.h"
28 
29 using namespace robot_model_services;
30 using namespace boost::unit_test;
31 
32 class IKTest : public BaseTest{
33 public:
34  IKTest() : BaseTest (false){
35  }
36 
37  virtual ~IKTest() {}
38 
39  void cameraPoseTest() {
40  std::vector<SimpleVector3> targetCameraPositions;
41  std::vector<SimpleQuaternion> targetCameraOrientations;
42 
43  //Initialize Poses
44  targetCameraPositions.push_back(SimpleVector3(0, 0, 0));
45  targetCameraOrientations.push_back(ZXZ2Quaternion(0.0, 0.0, 0.0));
46  //Added some poses where the X or Y direction of the camera is exactly zero
47  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
48  targetCameraOrientations.push_back(ZXZ2Quaternion(0, 0.0, 0 ));
49  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
50  targetCameraOrientations.push_back(ZXZ2Quaternion(90, 0.0, 0 ));
51  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
52  targetCameraOrientations.push_back(ZXZ2Quaternion(180, 0.0, 0 ));
53  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
54  targetCameraOrientations.push_back(ZXZ2Quaternion(270, 0.0, 0 ));
55 
56  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
57  targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 0.0, 0 ));
58  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
59  targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 80.0, 0));
60  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
61  targetCameraOrientations.push_back(ZXZ2Quaternion(-180, 100.0, 0 ));
62  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
63  targetCameraOrientations.push_back(ZXZ2Quaternion(-200, 40.0, 0));
64  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
65  targetCameraOrientations.push_back(ZXZ2Quaternion(-180, -30.0, 0));
66  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.4));
67  targetCameraOrientations.push_back(ZXZ2Quaternion(-190, -50.0, 0));
68  targetCameraPositions.push_back(SimpleVector3(-0.404993116856, -0.28920769691, 1.4));
69  targetCameraOrientations.push_back(ZXZ2Quaternion(-120, -20.0, 0));
70  targetCameraPositions.push_back(SimpleVector3(0.804993116856, 0.98920769691, 1.4));
71  targetCameraOrientations.push_back(ZXZ2Quaternion(120, -20.0, 0));
72  targetCameraPositions.push_back(SimpleVector3(0.004993116856, 2.08920769691, 1.5));
73  targetCameraOrientations.push_back(ZXZ2Quaternion(20, -20.0, 0));
74  targetCameraPositions.push_back(SimpleVector3(0.404993116856, 0.28920769691, 0.8));
75  targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 0.0, 0));
76  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.7));
77  targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 0.0, 0 ));
78  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -1.28920769691, 0.4));
79  targetCameraOrientations.push_back(ZXZ2Quaternion(-170, 80.0, 0));
80  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -1.28920769691, 0.5));
81  targetCameraOrientations.push_back(ZXZ2Quaternion(-180, 100.0, 0 ));
82  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.0));
83  targetCameraOrientations.push_back(ZXZ2Quaternion(-200, 40.0, 0));
84  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.8));
85  targetCameraOrientations.push_back(ZXZ2Quaternion(-180, -30.0, 0));
86  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.3));
87  targetCameraOrientations.push_back(ZXZ2Quaternion(-190, -50.0, 0));
88  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.1));
89  targetCameraOrientations.push_back(ZXZ2Quaternion(-120, -20.0, 0));
90  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.7));
91  targetCameraOrientations.push_back(ZXZ2Quaternion(120, -20.0, 0));
92  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 1.0));
93  targetCameraOrientations.push_back(ZXZ2Quaternion(20, -20.0, 0));
94  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 0.9));
95  targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 0.0, 0));
96  //a couple of very difficult test cases
97  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 2.5));
98  targetCameraOrientations.push_back(ZXZ2Quaternion(-20, -20.0, 0));
99  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, 0.0));
100  targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 30.0, 0));
101  targetCameraPositions.push_back(SimpleVector3(0.404993116856, -2.28920769691, -1.0));
102  targetCameraOrientations.push_back(ZXZ2Quaternion(-10, 30.0, 0));
103 
104 
105  ROS_INFO_STREAM("Initializing...");
106 
107  //Initialize robot model
108  robot_model_services::MILDRobotModelWithExactIK *myRobotModel = new robot_model_services::MILDRobotModelWithExactIK();
109  robot_model_services::MILDRobotModelWithExactIKPtr myRobotModelPtr(myRobotModel);
110  double position_x = -0.113948535919;
111  double position_y = -1.60322499275;
112  double rotationAngle = -1.57;
113 
114  robot_model_services::MILDRobotState * startState = new robot_model_services::MILDRobotState(0,0,rotationAngle,position_x,position_y);
115  robot_model_services::MILDRobotStatePtr startStatePtr(startState);
116 
117  //Publish current pose
118  ros::NodeHandle n;
119  ros::Publisher pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1000);
120  geometry_msgs::PoseWithCovarianceStamped startPoseStamped = geometry_msgs::PoseWithCovarianceStamped();
121  geometry_msgs::PoseWithCovariance startPose = geometry_msgs::PoseWithCovariance();
122  SimpleQuaternion rotation = ZXZ2Quaternion(0, 0, rotationAngle*180/M_PI);
123  startPose.pose.position.x = position_x;
124  startPose.pose.position.y = position_y;
125  startPose.pose.position.z = 0;
126  startPose.pose.orientation.w = rotation.w();
127  startPose.pose.orientation.x = rotation.x();
128  startPose.pose.orientation.y = rotation.y();
129  startPose.pose.orientation.z = rotation.z();
130  startPoseStamped.pose = startPose;
131  startPoseStamped.header.stamp = ros::Time::now();
132  pose_pub.publish(startPoseStamped);
133 
134 
135  myRobotModelPtr->setPanAngleLimits(-60, 60);
136  myRobotModelPtr->setTiltAngleLimits(-180, 180);
137  ROS_INFO_STREAM("Running test...");
138  ros::spinOnce();
139  for (unsigned int i = 0; i < targetCameraPositions.size(); i++)
140  {
141  ROS_INFO_STREAM("Testpose " << (i+1));
142  SimpleVector3 currentPosition = targetCameraPositions[i];
143  SimpleQuaternion currentOrientation = targetCameraOrientations[i];
144  ROS_INFO_STREAM("Calculating inverse kinematics...");
145  robot_model_services::RobotStatePtr newStatePtr = myRobotModelPtr->calculateRobotState(startStatePtr, currentPosition, currentOrientation);
146  //ROS_INFO_STREAM("Calculating camera pose correction...");
147  //robot_model_services::PTUConfig ptuConfig = myRobotModelPtr->calculateCameraPoseCorrection(startStatePtr, currentPosition, currentOrientation);
148  //ROS_INFO_STREAM("Got pan = " << std::get<0>(ptuConfig)*180/M_PI << ", tilt = " << std::get<1>(ptuConfig)*180/M_PI);
149  ros::spinOnce();
150  waitForEnter();
151  ros::Duration(2).sleep();
152  }
153  ROS_INFO_STREAM("All tests done.");
154  }
155 };
156 
157 test_suite* init_unit_test_suite( int argc, char* argv[] ) {
158  ros::init(argc, argv, "nbv_test");
159  ros::start();
160 
161  test_suite* evaluation = BOOST_TEST_SUITE("Evaluation NBV");
162 
163  boost::shared_ptr<IKTest> testPtr(new IKTest());
164 
165  evaluation->add(BOOST_CLASS_TEST_CASE(&IKTest::cameraPoseTest, testPtr));
166 
167  framework::master_test_suite().add(evaluation);
168 
169  return 0;
170 }
171 
ros::NodeHandle
ros::start
ROSCPP_DECL void start()
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::Duration::sleep
bool sleep() const
robot_model_services::MILDRobotModelWithExactIK
Definition: MILDRobotModelWithExactIK.hpp:41
IKTest::~IKTest
virtual ~IKTest()
Definition: IKTest.cpp:37
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
robot_model_services::SimpleVector3
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:50
boost::shared_ptr
IKTest::cameraPoseTest
void cameraPoseTest()
Definition: IKTest.cpp:39
unit_test
robot_model_services::MILDRobotState
Definition: MILDRobotState.hpp:26
robot_model_services
this namespace contains all generally usable classes.
Definition: DebugHelper.hpp:27
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
IKTest::IKTest
IKTest()
Definition: IKTest.cpp:34
init_unit_test_suite
test_suite * init_unit_test_suite(int argc, char *argv[])
Definition: IKTest.cpp:157
ros::Time::now
static Time now()
ros::spinOnce
ROSCPP_DECL void spinOnce()
robot_model_services::SimpleQuaternion
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:64

asr_robot_model_services
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Mon Jun 10 2019 12:49:59