From fb32a74090a90ec062b24e05932b88e3a8cea24c Mon Sep 17 00:00:00 2001
From: Mohammad Haghighipanah <mohammad.haghighipanah@intel.com>
Date: Mon, 15 Oct 2018 16:29:45 -0700
Subject: [PATCH] Adding unit test to Robot Class
---
nav2_robot/CMakeLists.txt | 3 +
nav2_robot/src/ros_robot.cpp | 2 +-
nav2_robot/test/CMakeLists.txt | 10 ++
nav2_robot/test/test_robot_class.cpp | 165 +++++++++++++++++++++++++++
4 files changed, 179 insertions(+), 1 deletion(-)
create mode 100644 nav2_robot/test/CMakeLists.txt
create mode 100644 nav2_robot/test/test_robot_class.cpp
diff --git a/nav2_robot/CMakeLists.txt b/nav2_robot/CMakeLists.txt
index f635ec94..7bde237c 100644
--- a/nav2_robot/CMakeLists.txt
+++ b/nav2_robot/CMakeLists.txt
@@ -42,8 +42,11 @@ install(DIRECTORY include/
)
if(BUILD_TESTING)
+ find_package(ament_cmake_pytest REQUIRED)
+ find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
+ add_subdirectory(test)
endif()
ament_export_include_directories(include)
diff --git a/nav2_robot/src/ros_robot.cpp b/nav2_robot/src/ros_robot.cpp
index 41be7555..2577c994 100644
--- a/nav2_robot/src/ros_robot.cpp
+++ b/nav2_robot/src/ros_robot.cpp
@@ -97,7 +97,7 @@ RosRobot::getCurrentVelocity(nav_msgs::msg::Odometry::SharedPtr & robot_velocity
return true;
}
-// TODO(mhpanah): modify this method name and implementation to include robot types and Serial # (ID)
+// TODO(mhpanah): modify this method name and implementation to include robot types and Serial #(ID)
std::string
RosRobot::getRobotName()
{
diff --git a/nav2_robot/test/CMakeLists.txt b/nav2_robot/test/CMakeLists.txt
new file mode 100644
index 00000000..2298461f
--- /dev/null
+++ b/nav2_robot/test/CMakeLists.txt
@@ -0,0 +1,10 @@
+include_directories( ${PROJECT_SOURCE_DIR}/src )
+
+ament_add_gtest(test_robot_class
+ test_robot_class.cpp
+)
+ament_target_dependencies(test_robot_class
+ ${dependencies}
+)
+target_link_libraries(test_robot_class nav2_robot)
+
diff --git a/nav2_robot/test/test_robot_class.cpp b/nav2_robot/test/test_robot_class.cpp
new file mode 100644
index 00000000..0540c72d
--- /dev/null
+++ b/nav2_robot/test/test_robot_class.cpp
@@ -0,0 +1,165 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <string>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_robot/ros_robot.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+using std::placeholders::_1;
+
+class RclCppFixture
+{
+public:
+ RclCppFixture() {rclcpp::init(0, nullptr);}
+ ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class TestRobotClass : public ::testing::Test
+{
+public:
+ TestRobotClass()
+ {
+ node = rclcpp::Node::make_shared("robot_class_test");
+ robot_ = std::make_unique<nav2_robot::RosRobot>(node.get());
+
+ pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("amcl_pose");
+ odom_pub_ = node->create_publisher<nav_msgs::msg::Odometry>("odom");
+
+ vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>("cmdVelocity",
+ std::bind(&TestRobotClass::velocityReceived, this, std::placeholders::_1));
+
+ velocityCmdReceived_ = false;
+ }
+ void velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+protected:
+ std::shared_ptr<rclcpp::Node> node;
+ std::unique_ptr<nav2_robot::RosRobot> robot_;
+ void publishPose();
+ void publishOdom();
+ geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
+ nav_msgs::msg::Odometry testOdom_;
+ geometry_msgs::msg::Twist velocityReceived_;
+ bool velocityCmdReceived_;
+
+private:
+ rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+ rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+ rclcpp::Subscription<geometry_msgs::msg::Twist>::ConstSharedPtr vel_sub_;
+};
+
+void
+TestRobotClass::publishPose()
+{
+ testPose_.header.frame_id = "map";
+ testPose_.header.stamp = rclcpp::Time();
+ testPose_.pose.pose.position.x = 1.1;
+ testPose_.pose.pose.position.y = 1.2;
+ testPose_.pose.pose.position.z = 1.3;
+ testPose_.pose.pose.orientation.x = 0.1;
+ testPose_.pose.pose.orientation.y = 0.2;
+ testPose_.pose.pose.orientation.z = 0.3;
+ testPose_.pose.pose.orientation.w = 0.4;
+ for (int i = 0; i < 12; i++) {
+ testPose_.pose.covariance[i] = i;
+ }
+ pose_pub_->publish(testPose_);
+}
+
+void
+TestRobotClass::publishOdom()
+{
+ testOdom_.header.frame_id = "base_footprint";
+ testOdom_.header.stamp = rclcpp::Time();
+
+ testOdom_.pose.pose.position.x = 1.1;
+ testOdom_.pose.pose.position.y = 1.2;
+ testOdom_.pose.pose.position.z = 1.3;
+ testOdom_.pose.pose.orientation.x = 0.1;
+ testOdom_.pose.pose.orientation.y = 0.2;
+ testOdom_.pose.pose.orientation.z = 0.3;
+ testOdom_.pose.pose.orientation.w = 0.4;
+ for (int i = 0; i < 12; i++) {
+ testOdom_.pose.covariance[i] = i;
+ }
+
+ testOdom_.twist.twist.linear.x = 1.1;
+ testOdom_.twist.twist.linear.y = 1.2;
+ testOdom_.twist.twist.linear.z = 1.3;
+ testOdom_.twist.twist.angular.x = 1.1;
+ testOdom_.twist.twist.angular.y = 1.2;
+ testOdom_.twist.twist.angular.z = 1.3;
+
+ for (int i = 0; i < 12; i++) {
+ testOdom_.twist.covariance[i] = i;
+ }
+
+ odom_pub_->publish(testOdom_);
+}
+
+void
+TestRobotClass::velocityReceived(const geometry_msgs::msg::Twist::SharedPtr msg)
+{
+ velocityReceived_ = *msg;
+ velocityCmdReceived_ = true;
+}
+
+TEST_F(TestRobotClass, getNameTest)
+{
+ std::string robotName = robot_->getRobotName();
+ EXPECT_EQ(robotName, "turtlebot");
+}
+
+TEST_F(TestRobotClass, getPoseTest)
+{
+ auto currentPose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
+ while (!(robot_->getCurrentPose(currentPose))) {
+ publishPose();
+ rclcpp::spin_some(node);
+ }
+ EXPECT_EQ(*currentPose, testPose_);
+}
+
+TEST_F(TestRobotClass, getVelocityTest)
+{
+ auto currentOdom = std::make_shared<nav_msgs::msg::Odometry>();
+ while (!(robot_->getCurrentVelocity(currentOdom))) {
+ publishOdom();
+ rclcpp::spin_some(node);
+ }
+ EXPECT_EQ(*currentOdom, testOdom_);
+}
+
+TEST_F(TestRobotClass, sendVelocityTest)
+{
+ geometry_msgs::msg::Twist velocity_cmd;
+ velocity_cmd.linear.x = 1.1;
+ velocity_cmd.linear.y = 1.2;
+ velocity_cmd.linear.z = 1.3;
+ velocity_cmd.angular.x = 1.1;
+ velocity_cmd.angular.y = 1.2;
+ velocity_cmd.angular.z = 1.3;
+
+ robot_->sendVelocity(velocity_cmd);
+
+ while (!velocityCmdReceived_) {
+ rclcpp::spin_some(node);
+ }
+ EXPECT_EQ(velocity_cmd, velocityReceived_);
+}
--
GitLab