collision_map_ground_plane_test.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author Sachin Chitta
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <message_filters/subscriber.h>
00041 #include <tf/transform_listener.h>
00042 #include <tf/message_filter.h>
00043 #include <boost/thread.hpp>
00044 #include <sys/time.h>
00045 
00046 #include <arm_navigation_msgs/OrientedBoundingBox.h>
00047 #include <arm_navigation_msgs/CollisionMap.h>
00048 
00049 #include <gtest/gtest.h>
00050 
00051 static const std::string COLLISION_MAP_TOPIC = "/collision_map_occ";
00052 static const std::string COLLISION_MAP_FRAME = "/odom";
00053 
00054 static const double MIN_Z_THRESHOLD = -0.1;
00055 static const double MAX_Z_THRESHOLD = 0.1;
00056 
00057 static const int TEST_NUM_MSGS = 2;
00058 
00059 namespace collision_map
00060 {
00061   class CollisionMapTest
00062   {
00063     public:
00064       ros::NodeHandle node_;
00065       ros::NodeHandle private_handle_;
00066       bool result_;
00067       int num_msgs_;
00068       int test_num_msgs_;
00069       tf::TransformListener tf_;
00070       std::string collision_map_frame_;
00071       double min_z_threshold_,max_z_threshold_;
00072       tf::MessageFilter<arm_navigation_msgs::CollisionMap>* cloud_notifier_;
00073       message_filters::Subscriber<arm_navigation_msgs::CollisionMap>* cloud_subscriber_;
00074 
00075       bool done_;
00076 
00077       CollisionMapTest(): private_handle_("~")
00078       {
00079         done_ = false;
00080         num_msgs_ = 0;
00081         result_ = true;
00082 
00083         //  ROS_INFO("Private handle: %s, %s",private_handle_.getName(), private_handle_.getNamespace());
00084 
00085         private_handle_.param<double>("min_z_threshold",min_z_threshold_,MIN_Z_THRESHOLD);
00086         private_handle_.param<double>("max_z_threshold",max_z_threshold_,MAX_Z_THRESHOLD);
00087         private_handle_.param<int>("test_num_collision_msgs",test_num_msgs_,TEST_NUM_MSGS);
00088 
00089         private_handle_.param<std::string>("collision_map_frame",collision_map_frame_,COLLISION_MAP_FRAME);
00090 
00091         cloud_subscriber_ = new message_filters::Subscriber<arm_navigation_msgs::CollisionMap>(node_,COLLISION_MAP_TOPIC,50);
00092         cloud_notifier_ = new tf::MessageFilter<arm_navigation_msgs::CollisionMap>(*cloud_subscriber_,tf_,collision_map_frame_,50);
00093         cloud_notifier_->registerCallback(boost::bind(&CollisionMapTest::collisionCallback,this,_1));
00094 
00095       }
00096 
00097       void collisionCallback(const arm_navigation_msgs::CollisionMapConstPtr& msg)
00098       {        
00099         num_msgs_++;
00100         if(num_msgs_ < test_num_msgs_)
00101         {
00102           ROS_INFO("Got collision map update %d",num_msgs_);
00103           ROS_INFO("Waiting for %d updates before running test\n",test_num_msgs_);
00104           return;
00105         }
00106         ROS_INFO("Got collision map update %d, Running test.",num_msgs_);
00107 
00108         arm_navigation_msgs::CollisionMap map = *msg;
00109         for(int i=0; i < (int) map.boxes.size(); i++)
00110         {
00111           if(map.boxes[i].center.z > max_z_threshold_ || map.boxes[i].center.z < min_z_threshold_)
00112           {
00113             result_ = false;
00114             break;
00115           }
00116         }
00117         EXPECT_TRUE(result_);
00118         done_ = true;
00119       }      
00120 };
00121 }
00122 
00123 void spinThread()
00124 {
00125   ros::spin();
00126 }
00127 
00128 TEST(CollisionMapTest, collisionMapGroundPlaneTest)
00129 {
00130   boost::thread spin_thread(&spinThread);
00131   collision_map::CollisionMapTest cmap;
00132   ros::Duration dd(1.0);   
00133   ros::NodeHandle nh;
00134 
00135   while (nh.ok() && !cmap.done_)
00136   {
00137     dd.sleep();
00138   }
00139   ros::shutdown();
00140   spin_thread.join();
00141 }
00142 
00143 int main(int argc, char **argv){
00144   testing::InitGoogleTest(&argc, argv);
00145   ros::init (argc, argv, "collision_map_test");
00146   return RUN_ALL_TESTS();
00147 }


collision_map
Author(s): Radu Bogdan Rusu, Ioan Sucan
autogenerated on Mon Dec 2 2013 12:38:32