set_model_state_test.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gazebo_msgs/ModelState.h>
3 #include <gazebo_msgs/ModelStates.h>
4 
5 #include <gtest/gtest.h>
6 #include <iostream>
7 
8 #include <tf/tf.h>
9 
10 double curX_;
11 double curY_;
12 double curHeading_;
14 
15 void modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
16 {
17  // Find the robot's state, update pose variables
18  size_t modelCount = msg->name.size();
19  tf::Quaternion quat;
20  double dummy;
21 
22  for(size_t modelInd = 0; modelInd < modelCount; ++modelInd)
23  {
24  if(msg->name[modelInd] == "pioneer2dx")
25  {
26  vehicleSpawned_ = true;
27 
28  tf::quaternionMsgToTF(msg->pose[modelInd].orientation, quat);
29  tf::Matrix3x3 mat(quat);
30  mat.getRPY(dummy, dummy, curHeading_);
31  curX_ = msg->pose[modelInd].position.x;
32  curY_ = msg->pose[modelInd].position.y;
33 
34  break;
35  }
36  }
37 }
38 
39 TEST (ModelStateTest, FrameTest)
40 {
41  vehicleSpawned_ = false;
42  curHeading_ = 0;
43 
44  ros::NodeHandle nh;
45  ros::Publisher modelStatePub = nh.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 5);
46  ros::Subscriber modelStatesSub = nh.subscribe("/gazebo/model_states", 1, &modelStatesCallback);
47 
48  gazebo_msgs::ModelState state;
49 
50  // Issue commands in the chassis frame
51  state.model_name = "pioneer2dx";
52  state.pose.orientation.w = 1.0;
53  state.reference_frame = "pioneer2dx::chassis";
54 
55  // Wait for model to spawn
56  while(!vehicleSpawned_)
57  {
58  ros::spinOnce();
59  ros::Duration(0.1).sleep();
60  }
61 
62  // First, turn the robot so it's going about 45 degrees
63  state.pose.orientation.z = 0.04018;
64  do
65  {
66  modelStatePub.publish(state);
67  ros::Duration(0.2).sleep();
68  ros::spinOnce();
69  } while(::fabs(curHeading_ - M_PI/4) > 0.05);
70 
71  state.pose.orientation.z = 0;
72  state.pose.position.x = 0.1;
73 
74  // Now, stop the robot and drive forwards about 5 meters
75  do
76  {
77  modelStatePub.publish(state);
78  ros::Duration(0.2).sleep();
79  ros::spinOnce();
80  } while(::sqrt(curX_*curX_ + curY_*curY_) < 5.0);
81 
82  // The X and Y values should be approximately the same,
83  // and should be roughly sqrt(25/2)
84  EXPECT_LT(::fabs(curX_ - 3.535533906), 0.2);
85  EXPECT_LT(::fabs(curY_ - 3.535533906), 0.2);
86 
87  state.pose.position.x = 0;
88  state.reference_frame = "world";
89 
90  // Now, send the robot to (0, 0) in the world frame
91  do
92  {
93  modelStatePub.publish(state);
94  ros::Duration(0.2).sleep();
95  ros::spinOnce();
96  } while(::sqrt(curX_*curX_ + curY_*curY_) > 0.1);
97 
98  EXPECT_LT(::fabs(curX_), 0.01);
99  EXPECT_LT(::fabs(curY_), 0.01);
100 }
101 
102 int main(int argc, char **argv)
103 {
104  testing::InitGoogleTest(&argc, argv);
105 
106  ros::init(argc, argv, "set_model_state-test");
107  ros::Time::init();
108 
109  return RUN_ALL_TESTS();
110 }
double curHeading_
bool vehicleSpawned_
void publish(const boost::shared_ptr< M > &message) const
void modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double curX_
int main(int argc, char **argv)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
double curY_
static void init()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TEST(ModelStateTest, FrameTest)
ROSCPP_DECL void spinOnce()


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39