moveit_config_data_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018, Mohamad Ayman.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mohamad Ayman */
36 #include <gtest/gtest.h>
37 #include <sstream>
38 #include <ctype.h>
39 #include <iostream> // For writing yaml and launch files
40 #include <urdf_parser/urdf_parser.h>
41 #include <fstream>
42 #include <string>
43 #include <boost/filesystem/path.hpp>
44 #include <moveit_resources/config.h>
45 #include <boost/filesystem.hpp> // for creating/deleting folders/files
47 
48 // This tests writing/parsing of ros_controller.yaml
49 class MoveItConfigData : public testing::Test
50 {
51 protected:
52  void SetUp() override
53  {
54  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
55 
56  srdf_model.reset(new srdf::Model());
57  std::string xml_string;
58  std::fstream xml_file((res_path / "panda_description/urdf/panda.urdf").string().c_str(), std::fstream::in);
59  if (xml_file.is_open())
60  {
61  while (xml_file.good())
62  {
63  std::string line;
64  std::getline(xml_file, line);
65  xml_string += (line + "\n");
66  }
67  xml_file.close();
68  urdf_model = urdf::parseURDF(xml_string);
69  }
70  srdf_model->initFile(*urdf_model, (res_path / "panda_moveit_config/config/panda.srdf").string());
72  };
73 
74 protected:
75  urdf::ModelInterfaceSharedPtr urdf_model;
77  moveit::core::RobotModelPtr robot_model;
78 };
79 
80 TEST_F(MoveItConfigData, ReadingControllers)
81 {
82  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
83 
84  // Contains all the configuration data for the setup assistant
85  moveit_setup_assistant::MoveItConfigDataPtr config_data_;
86 
87  config_data_.reset(new moveit_setup_assistant::MoveItConfigData());
88  config_data_->srdf_->srdf_model_ = srdf_model;
89  config_data_->setRobotModel(robot_model);
90 
91  // Initially no controllers
92  EXPECT_EQ(config_data_->getROSControllers().size(), 0);
93 
94  // Adding default controllers, a controller for each planning group
95  config_data_->addDefaultControllers();
96 
97  // Number of the planning groups defined in the model srdf
98  int group_count = config_data_->srdf_->srdf_model_->getGroups().size();
99 
100  // Test that addDefaultControllers() did accually add a controller for the new_group
101  EXPECT_EQ(config_data_->getROSControllers().size(), group_count);
102 
103  // Temporary file used during the test and is deleted when the test is finished
104  char test_file[] = "/tmp/msa_unittest_ros_controller.yaml";
105 
106  // ros_controller.yaml written correctly
107  EXPECT_EQ(config_data_->outputROSControllersYAML(test_file), true);
108 
109  // Reset moveit config MoveItConfigData
110  config_data_.reset(new moveit_setup_assistant::MoveItConfigData());
111 
112  // Initially no controllers
113  EXPECT_EQ(config_data_->getROSControllers().size(), 0);
114 
115  // ros_controllers.yaml read correctly
116  EXPECT_EQ(config_data_->inputROSControllersYAML(test_file), true);
117 
118  // ros_controllers.yaml parsed correctly
119  EXPECT_EQ(config_data_->getROSControllers().size(), group_count);
120 
121  // Remove ros_controllers.yaml temp file which was used in testing
122  boost::filesystem::remove(test_file);
123 }
124 
125 // This tests parsing of sensors_3d.yaml
126 TEST_F(MoveItConfigData, ReadingSensorsConfig)
127 {
128  // Contains all the config data for the setup assistant
129  moveit_setup_assistant::MoveItConfigDataPtr config_data_;
130  config_data_.reset(new moveit_setup_assistant::MoveItConfigData());
131 
132  boost::filesystem::path setup_assistant_path(config_data_->setup_assistant_path_);
133 
134  // Before parsing, no config available
135  EXPECT_EQ(config_data_->getSensorPluginConfig().size(), 0);
136 
137  // Read the file containing the default config parameters
138  config_data_->input3DSensorsYAML(
139  (setup_assistant_path / "templates/moveit_config_pkg_template/config/sensors_3d.yaml").string());
140 
141  // Default config for the two available sensor plugins
142  // Make sure both are parsed correctly
143  EXPECT_EQ(config_data_->getSensorPluginConfig().size(), 2);
144 
145  EXPECT_EQ(config_data_->getSensorPluginConfig()[0]["sensor_plugin"].getValue(),
146  std::string("occupancy_map_monitor/PointCloudOctomapUpdater"));
147 
148  EXPECT_EQ(config_data_->getSensorPluginConfig()[1]["sensor_plugin"].getValue(),
149  std::string("occupancy_map_monitor/DepthImageOctomapUpdater"));
150 }
151 
152 int main(int argc, char** argv)
153 {
154  testing::InitGoogleTest(&argc, argv);
155  return RUN_ALL_TESTS();
156 }
int main(int argc, char **argv)
#define EXPECT_EQ(a, b)
def xml_string(rootXml, addHeader=True)
moveit::core::RobotModelPtr robot_model
urdf::ModelInterfaceSharedPtr urdf_model
srdf::ModelSharedPtr srdf_model
This class is shared with all widgets and contains the common configuration data needed for generatin...
TEST_F(MoveItConfigData, ReadingControllers)


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28