From be38a7a91811e27742ca4be01dbef16436a461fe Mon Sep 17 00:00:00 2001
From: Wilco Bonestroo <w.j.bonestroo@saxion.nl>
Date: Mon, 12 Oct 2020 20:58:44 +0200
Subject: [PATCH] Add kinematic parameters test (#2031)
* Add kinematic parameters tests.
* Solve uncrustify error.
---
.../dwb_plugins/test/CMakeLists.txt | 3 +
.../test/kinematic_parameters_test.cpp | 170 ++++++++++++++++++
2 files changed, 173 insertions(+)
create mode 100644 nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp
diff --git a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
index 29845a84..f7eb628d 100644
--- a/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
+++ b/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt
@@ -2,3 +2,6 @@ ament_add_gtest(vtest velocity_iterator_test.cpp)
ament_add_gtest(twist_gen_test twist_gen.cpp)
target_link_libraries(twist_gen_test standard_traj_generator)
+
+ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp)
+target_link_libraries(kinematic_parameters_test standard_traj_generator)
diff --git a/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp b/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp
new file mode 100644
index 00000000..7e9611cc
--- /dev/null
+++ b/nav2_dwb_controller/dwb_plugins/test/kinematic_parameters_test.cpp
@@ -0,0 +1,170 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2018, Wilco Bonestroo
+ * 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 copyright holder 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 HOLDER 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 <string>
+#include <memory>
+
+#include "gtest/gtest.h"
+#include "dwb_plugins/kinematic_parameters.hpp"
+
+using rcl_interfaces::msg::Parameter;
+using rcl_interfaces::msg::ParameterType;
+using rcl_interfaces::msg::ParameterEvent;
+
+class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
+{
+public:
+ void simulate_event(
+ const ParameterEvent::SharedPtr event)
+ {
+ on_parameter_event_callback(event);
+ }
+};
+
+TEST(KinematicParameters, SetAllParameters) {
+ std::string nodeName = "test_node";
+ auto node = nav2_util::LifecycleNode::make_shared(nodeName);
+ KinematicsHandlerTest kh;
+ kh.initialize(node, nodeName);
+
+ rcl_interfaces::msg::ParameterEvent event;
+ Parameter p_minX, p_maxX, p_minY, p_maxY, p_accX, p_decelX, p_accY, p_decelY, p_minSpeedXY,
+ p_maxSpeedXY, p_maxTheta, p_accTheta, p_decelTheta, p_minSpeedTheta;
+
+ p_minX.name = nodeName + ".min_vel_x";
+ p_minX.value.set__double_value(12.34);
+ p_minX.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_maxX.name = nodeName + ".max_vel_x";
+ p_maxX.value.set__double_value(23.45);
+ p_maxX.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_minY.name = nodeName + ".min_vel_y";
+ p_minY.value.set__double_value(34.56);
+ p_minY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_maxY.name = nodeName + ".max_vel_y";
+ p_maxY.value.set__double_value(45.67);
+ p_maxY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_accX.name = nodeName + ".acc_lim_x";
+ p_accX.value.set__double_value(56.78);
+ p_accX.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_accY.name = nodeName + ".acc_lim_y";
+ p_accY.value.set__double_value(67.89);
+ p_accY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_decelX.name = nodeName + ".decel_lim_x";
+ p_decelX.value.set__double_value(78.90);
+ p_decelX.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_decelY.name = nodeName + ".decel_lim_y";
+ p_decelY.value.set__double_value(89.01);
+ p_decelY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_minSpeedXY.name = nodeName + ".min_speed_xy";
+ p_minSpeedXY.value.set__double_value(90.12);
+ p_minSpeedXY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_maxSpeedXY.name = nodeName + ".max_speed_xy";
+ p_maxSpeedXY.value.set__double_value(123.456);
+ p_maxSpeedXY.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_maxTheta.name = nodeName + ".max_vel_theta";
+ p_maxTheta.value.set__double_value(345.678);
+ p_maxTheta.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_accTheta.name = nodeName + ".acc_lim_theta";
+ p_accTheta.value.set__double_value(234.567);
+ p_accTheta.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_decelTheta.name = nodeName + ".decel_lim_theta";
+ p_decelTheta.value.set__double_value(456.789);
+ p_decelTheta.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ p_minSpeedTheta.name = nodeName + ".min_speed_theta";
+ p_minSpeedTheta.value.set__double_value(567.890);
+ p_minSpeedTheta.value.set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE);
+
+ event.changed_parameters.push_back(p_minX);
+ event.changed_parameters.push_back(p_maxX);
+ event.changed_parameters.push_back(p_minY);
+ event.changed_parameters.push_back(p_maxY);
+ event.changed_parameters.push_back(p_accX);
+ event.changed_parameters.push_back(p_accY);
+ event.changed_parameters.push_back(p_decelX);
+ event.changed_parameters.push_back(p_decelY);
+ event.changed_parameters.push_back(p_minSpeedXY);
+ event.changed_parameters.push_back(p_maxSpeedXY);
+ event.changed_parameters.push_back(p_maxTheta);
+ event.changed_parameters.push_back(p_accTheta);
+ event.changed_parameters.push_back(p_decelTheta);
+ event.changed_parameters.push_back(p_minSpeedTheta);
+
+ kh.simulate_event(std::make_shared<ParameterEvent>(event));
+
+ dwb_plugins::KinematicParameters kp = kh.getKinematics();
+
+ EXPECT_EQ(kp.getMinX(), 12.34);
+ EXPECT_EQ(kp.getMaxX(), 23.45);
+ EXPECT_EQ(kp.getMinY(), 34.56);
+ EXPECT_EQ(kp.getMaxY(), 45.67);
+ EXPECT_EQ(kp.getAccX(), 56.78);
+ EXPECT_EQ(kp.getAccY(), 67.89);
+ EXPECT_EQ(kp.getDecelX(), 78.90);
+ EXPECT_EQ(kp.getDecelY(), 89.01);
+ EXPECT_EQ(kp.getMinSpeedXY(), 90.12);
+ EXPECT_EQ(kp.getMaxSpeedXY(), 123.456);
+ EXPECT_EQ(kp.getAccTheta(), 234.567);
+ EXPECT_EQ(kp.getMaxTheta(), 345.678);
+ EXPECT_EQ(kp.getDecelTheta(), 456.789);
+ EXPECT_EQ(kp.getMinSpeedTheta(), 567.890);
+}
+
+
+int main(int argc, char ** argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+
+ // initialize ROS
+ rclcpp::init(argc, argv);
+
+ bool all_successful = RUN_ALL_TESTS();
+
+ // shutdown ROS
+ rclcpp::shutdown();
+
+ return all_successful;
+}
--
GitLab