test_dynamic_planner.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
#include <boost/bind.hpp>
#include <boost/scoped_ptr.hpp>

#include "ros/ros.h"

#include <gtest/gtest.h>

using namespace tf;


void getPose(tf::TransformListener* listener, double* x, double* y, double* th){
  geometry_msgs::TransformStamped geo_pose;
  tf::StampedTransform transform;
  try{
    listener->lookupTransform("/map", "/robot_0/base_link", ros::Time(0), transform);

    transformStampedTFToMsg(transform, geo_pose);

    *x = geo_pose.transform.translation.x;
    *y = geo_pose.transform.translation.y;
    *th = tf::getYaw(geo_pose.transform.rotation);
  }
  catch (tf::TransformException ex){
    //ROS_ERROR("%s",ex.what());
    *x=0;
    *y=0;
    *th=0;
  }

}

TEST(dynamicPlanner, plannerFailure)
{
  tf::TransformListener listener;

  double goal_x = 20.413;
  double goal_y = 6.913;

  ros::NodeHandle n;
  double x,y,th;
  double t = ros::Time::now().toSec();
  int atGoal = 0;

  sleep(5.0);

  while(ros::Time::now().toSec()-t < 180.0 && !atGoal){
    getPose(&listener,&x,&y,&th);
    ROS_INFO("x=%f y=%f",x,y);
    double dx = goal_x-x;
    double dy = goal_y-y;
    double dist = sqrt(dx*dx+dy*dy);
    atGoal = dist < 2.0;
    ros::Duration(2.0).sleep();
  }

  EXPECT_EQ(1, atGoal);
}

int main(int argc, char** argv)
{
        testing::InitGoogleTest(&argc, argv);

        ros::Time::setNow(ros::Time());
        ros::init(argc, argv, "test_dynamic_planner");
        ros::NodeHandle nh;

        int ret = RUN_ALL_TESTS();

        return ret;
}

---

demo_sbpl_dynamic_env
Author(s): Michael Phillips
autogenerated on Fri Jan 11 09:37:33 2013