set_model_state_test.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <gazebo_msgs/ModelState.h>
00003 #include <gazebo_msgs/ModelStates.h>
00004 
00005 #include <gtest/gtest.h>
00006 #include <iostream>
00007 
00008 #include <tf/tf.h>
00009 
00010 double curX_;
00011 double curY_;
00012 double curHeading_;
00013 bool vehicleSpawned_;
00014 
00015 void modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
00016 { 
00017   // Find the robot's state, update pose variables
00018   size_t modelCount = msg->name.size();
00019   tf::Quaternion quat;
00020   double dummy;
00021   
00022   for(size_t modelInd = 0; modelInd < modelCount; ++modelInd)
00023   {
00024     if(msg->name[modelInd] == "pioneer2dx")
00025     {
00026       vehicleSpawned_ = true;
00027 
00028       tf::quaternionMsgToTF(msg->pose[modelInd].orientation, quat);
00029       tf::Matrix3x3 mat(quat);
00030       mat.getRPY(dummy, dummy, curHeading_);
00031       curX_ = msg->pose[modelInd].position.x;
00032       curY_ = msg->pose[modelInd].position.y;
00033 
00034       break;
00035     }
00036   }
00037 }
00038 
00039 TEST (ModelStateTest, FrameTest)
00040 { 
00041   vehicleSpawned_ = false;
00042   curHeading_ = 0;
00043 
00044   ros::NodeHandle nh;
00045   ros::Publisher modelStatePub = nh.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 5);
00046   ros::Subscriber modelStatesSub = nh.subscribe("/gazebo/model_states", 1, &modelStatesCallback);
00047 
00048   gazebo_msgs::ModelState state;
00049 
00050   // Issue commands in the chassis frame
00051   state.model_name = "pioneer2dx";
00052   state.pose.orientation.w = 1.0;
00053   state.reference_frame = "pioneer2dx::chassis";
00054 
00055   // Wait for model to spawn
00056   while(!vehicleSpawned_)
00057   {
00058     ros::spinOnce();
00059     ros::Duration(0.1).sleep();
00060   }
00061 
00062   // First, turn the robot so it's going about 45 degrees
00063   state.pose.orientation.z = 0.04018;
00064   do
00065   {
00066     modelStatePub.publish(state);
00067     ros::Duration(0.2).sleep();
00068     ros::spinOnce();
00069   } while(::fabs(curHeading_ - M_PI/4) > 0.05);
00070 
00071   state.pose.orientation.z = 0;
00072   state.pose.position.x = 0.1;
00073 
00074   // Now, stop the robot and drive forwards about 5 meters
00075   do
00076   {
00077     modelStatePub.publish(state);
00078     ros::Duration(0.2).sleep();
00079     ros::spinOnce();
00080   } while(::sqrt(curX_*curX_ + curY_*curY_) < 5.0);
00081 
00082   // The X and Y values should be approximately the same,
00083   // and should be roughly sqrt(25/2)
00084   EXPECT_LT(::fabs(curX_ - 3.535533906), 0.2);
00085   EXPECT_LT(::fabs(curY_ - 3.535533906), 0.2);
00086 
00087   state.pose.position.x = 0;
00088   state.reference_frame = "world";
00089 
00090   // Now, send the robot to (0, 0) in the world frame
00091   do
00092   {
00093     modelStatePub.publish(state);
00094     ros::Duration(0.2).sleep();
00095     ros::spinOnce();
00096   } while(::sqrt(curX_*curX_ + curY_*curY_) > 0.1);
00097 
00098   EXPECT_LT(::fabs(curX_), 0.01);
00099   EXPECT_LT(::fabs(curY_), 0.01);
00100 }
00101 
00102 int main(int argc, char **argv)
00103 {
00104   testing::InitGoogleTest(&argc, argv);
00105 
00106   ros::init(argc, argv, "set_model_state-test");
00107   ros::Time::init();
00108 
00109   return RUN_ALL_TESTS();
00110 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23