sparse_planner.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Apache License)
00003  *
00004  * Copyright (c) 2014, Southwest Research Institute
00005  *
00006  * Licensed under the Apache License, Version 2.0 (the "License");
00007  * you may not use this file except in compliance with the License.
00008  * You may obtain a copy of the License at
00009  *
00010  * http://www.apache.org/licenses/LICENSE-2.0
00011  *
00012  * Unless required by applicable law or agreed to in writing, software
00013  * distributed under the License is distributed on an "AS IS" BASIS,
00014  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00015  * See the License for the specific language governing permissions and
00016  * limitations under the License.
00017  */
00018 
00019 #include <ros/node_handle.h>
00020 #include <descartes_planner/sparse_planner.h>
00021 #include <descartes_trajectory/cart_trajectory_pt.h>
00022 #include <descartes_core/utils.h>
00023 #include <descartes_trajectory_test/cartesian_robot.h>
00024 #include <gtest/gtest.h>
00025 #include <descartes_core/pretty_print.hpp>
00026 #include <tuple>
00027 
00028 using namespace descartes_core;
00029 using namespace descartes_trajectory;
00030 typedef std::vector<descartes_core::TrajectoryPtPtr> Trajectory;
00031 const int NUM_DENSE_POINTS = 1000;
00032 Trajectory createTestTrajectory();
00033 Trajectory TEST_TRAJECTORY = createTestTrajectory();
00034 descartes_planner::SparsePlanner Planner;
00035 
00036 class ThreeDOFRobot: public descartes_trajectory_test::CartesianRobot
00037 {
00038 public:
00039   ThreeDOFRobot():
00040     descartes_trajectory_test::CartesianRobot(0,0,3)
00041   {
00042 
00043   }
00044 
00045   virtual ~ThreeDOFRobot()
00046   {
00047 
00048   }
00049 
00050 };
00051 
00052 class TestPoint: public descartes_trajectory::CartTrajectoryPt
00053 {
00054 public:
00055   TestPoint(const std::vector<double>& joints)
00056   {
00057     vals_.resize(joints.size());
00058     vals_.assign(joints.begin(),joints.end());
00059   }
00060 
00061   virtual ~TestPoint()
00062   {
00063 
00064   }
00065 
00066   virtual bool getClosestJointPose(const std::vector<double> &seed_state,
00067                                      const RobotModel &model,
00068                                      std::vector<double> &joint_pose) const
00069   {
00070     joint_pose.clear();
00071     joint_pose.assign(vals_.begin(),vals_.end());
00072     return true;
00073   }
00074 
00075   virtual void getJointPoses(const RobotModel &model,
00076                                        std::vector<std::vector<double> > &joint_poses) const
00077   {
00078     joint_poses.clear();
00079     joint_poses.push_back(vals_);
00080 
00081   }
00082 
00083 protected:
00084 
00085   std::vector<double> vals_;
00086 };
00087 
00088 Trajectory createTestTrajectory()
00089 {
00090   ROS_INFO_STREAM("Creating test trajectory with "<<NUM_DENSE_POINTS<<" points");
00091   Trajectory traj;
00092   std::vector<std::tuple<double, double>>joint_bounds = {std::make_tuple(0,M_PI),
00093                                                          std::make_tuple(-M_PI_2,M_PI_2),
00094                                                          std::make_tuple(M_PI/8,M_PI/3)};
00095   std::vector<double> deltas;
00096   for(auto& e:joint_bounds)
00097   {
00098     double d = (std::get<1>(e)- std::get<0>(e))/NUM_DENSE_POINTS;
00099     deltas.push_back(d);
00100   }
00101 
00102   // creating trajectory points
00103   std::vector<double> joint_vals(deltas.size(),0);
00104   traj.reserve(NUM_DENSE_POINTS);
00105   for(int i = 0 ; i < NUM_DENSE_POINTS; i++)
00106   {
00107     for(int j = 0; j < deltas.size(); j++)
00108     {
00109       joint_vals[j] = std::get<0>(joint_bounds[j]) + deltas[j]*i;
00110     }
00111 
00112     TrajectoryPtPtr tp(new TestPoint(joint_vals));
00113     traj.push_back(tp);
00114   }
00115   return traj;
00116 }
00117 
00118 TEST(SparsePlanner, initialize)
00119 {
00120   ros::Time::init();
00121   RobotModelConstPtr robot(new ThreeDOFRobot());
00122   EXPECT_TRUE(Planner.initialize(robot));
00123 }
00124 
00125 TEST(SparsePlanner, configure)
00126 {
00127   descartes_core::PlannerConfig config;
00128   Planner.getConfig(config);
00129   EXPECT_TRUE(Planner.setConfig(config));
00130 }
00131 
00132 
00133 TEST(SparsePlanner, planPath)
00134 {
00135   ROS_INFO_STREAM("Testing planPath() with "<<NUM_DENSE_POINTS<<" points");
00136   EXPECT_TRUE(Planner.planPath(TEST_TRAJECTORY));
00137 }
00138 
00139 TEST(SparsePlanner, getPath)
00140 {
00141   std::vector<descartes_core::TrajectoryPtPtr> path;
00142   EXPECT_TRUE(Planner.getPath(path));
00143   EXPECT_TRUE(path.size() == NUM_DENSE_POINTS);
00144 }
00145 
00146 


descartes_planner
Author(s): Jorge Nicho
autogenerated on Wed Aug 26 2015 11:21:35