WIP: test base hardware interface (#30)

diffbot_base/test/diffbot_base.test

@@ -0,0 +1,13 @@
+<launch>
+ <!-- Load common test stuff -->
+ <include file="$(find diffbot_base)/test/diffbot_common.launch" />
+
+ <!-- Controller test -->
+ <test test-name="diffbot_base_test"
+ pkg="diffbot_base"
+ type="diffbot_base_test"
+ time-limit="80.0">
+ <remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
+ <remap from="odom" to="diffbot_controller/odom" />
+ </test>
+</launch>

diffbot_base/test/diffbot_base_test.cpp

@@ -0,0 +1,154 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, 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.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Bence Magyar
+
+#include "test_common.h"
+#include <tf/transform_listener.h>
+
+// TEST CASES
+TEST_F(DiffDriveControllerTest, testForward)
+{
+ // wait for ROS
+ waitForController();
+
+ // zero everything before test
+ geometry_msgs::Twist cmd_vel;
+ cmd_vel.linear.x = 0.0;
+ cmd_vel.angular.z = 0.0;
+ publish(cmd_vel);
+ ros::Duration(2.0).sleep();
+ // get initial odom
+ nav_msgs::Odometry old_odom = getLastOdom();
+ // send a velocity command of 0.1 m/s
+ cmd_vel.linear.x = 0.1;
+ publish(cmd_vel);
+ // wait for 10s
+ ros::Duration(10.0).sleep();
+
+ nav_msgs::Odometry new_odom = getLastOdom();
+
+ const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
+
+ const double expected_distance = cmd_vel.linear.x * actual_elapsed_time.toSec();
+
+ // check if the robot traveled 1 meter in XY plane, changes in z should be ~~0
+ const double dx = new_odom.pose.pose.position.x - old_odom.pose.pose.position.x;
+ const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y;
+ const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z;
+ EXPECT_NEAR(sqrt(dx*dx + dy*dy), expected_distance, POSITION_TOLERANCE);
+ EXPECT_LT(fabs(dz), EPS);
+
+ // convert to rpy and test that way
+ double roll_old, pitch_old, yaw_old;
+ double roll_new, pitch_new, yaw_new;
+ tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
+ tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
+ EXPECT_LT(fabs(roll_new - roll_old), EPS);
+ EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
+ EXPECT_LT(fabs(yaw_new - yaw_old), EPS);
+ EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
+
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS);
+}
+
+TEST_F(DiffDriveControllerTest, testTurn)
+{
+ // wait for ROS
+ waitForController();
+
+ // zero everything before test
+ geometry_msgs::Twist cmd_vel;
+ cmd_vel.linear.x = 0.0;
+ cmd_vel.angular.z = 0.0;
+ publish(cmd_vel);
+ ros::Duration(2.0).sleep();
+ // get initial odom
+ nav_msgs::Odometry old_odom = getLastOdom();
+ // send a velocity command
+ cmd_vel.angular.z = M_PI/10.0;
+ publish(cmd_vel);
+ // wait for 10s
+ ros::Duration(10.0).sleep();
+
+ nav_msgs::Odometry new_odom = getLastOdom();
+
+ // check if the robot rotated PI around z, changes in the other fields should be ~~0
+ EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS);
+ EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS);
+ EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS);
+
+ const ros::Duration actual_elapsed_time = new_odom.header.stamp - old_odom.header.stamp;
+ const double expected_rotation = cmd_vel.angular.z * actual_elapsed_time.toSec();
+
+ // convert to rpy and test that way
+ double roll_old, pitch_old, yaw_old;
+ double roll_new, pitch_new, yaw_new;
+ tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old);
+ tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new);
+ EXPECT_LT(fabs(roll_new - roll_old), EPS);
+ EXPECT_LT(fabs(pitch_new - pitch_old), EPS);
+ EXPECT_NEAR(fabs(yaw_new - yaw_old), expected_rotation, ORIENTATION_TOLERANCE);
+
+ EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS);
+
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS);
+ EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS);
+ EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), expected_rotation/actual_elapsed_time.toSec(), EPS);
+}
+
+TEST_F(DiffDriveControllerTest, testOdomFrame)
+{
+ // wait for ROS
+ waitForController();
+
+ // set up tf listener
+ tf::TransformListener listener;
+ ros::Duration(2.0).sleep();
+ // check the odom frame exist
+ EXPECT_TRUE(listener.frameExists("odom"));
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ ros::init(argc, argv, "diff_drive_test");
+
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+ //ros::Duration(0.5).sleep();
+ int ret = RUN_ALL_TESTS();
+ spinner.stop();
+ ros::shutdown();
+ return ret;
+}

diffbot_base/test/diffbot_common.launch

@@ -0,0 +1,33 @@
+<launch>
+ <!-- Use simulation time -->
+ <rosparam param="use_sim_time">True</rosparam>
+
+ <!-- Load diffbot model -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro '$(find diffbot_description)/urdf/diffbot.urdf.xacro'" />
+
+ <!-- Start diffbot_base -->
+ <node name="diffbot"
+ pkg="diffbot_base"
+ type="diffbot_base"/>
+
+ <!-- Load controller config -->
+ <rosparam command="load" file="$(find diffbot_control)/config/diffbot_control.yaml" />
+
+ <!-- Spawn controller -->
+ <node name="controller_spawner"
+ pkg="controller_manager" type="spawner" output="screen"
+ args="diffbot_controller" />
+
+ <!-- rqt_plot monitoring -->
+ <!--
+ <node name="diffbot_pos_monitor"
+ pkg="rqt_plot"
+ type="rqt_plot"
+ args="/diffbot_controller/odom/pose/pose/position/x" />
+ <node name="diffbot_vel_monitor"
+ pkg="rqt_plot"
+ type="rqt_plot"
+ args="/diffbot_controller/odom/twist/twist/linear/x" />
+ -->
+</launch>

diffbot_base/test/test_common.h

@@ -0,0 +1,162 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, 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.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Bence Magyar
+
+#pragma once
+
+
+#include <cmath>
+
+#include <gtest/gtest.h>
+
+#include <ros/ros.h>
+
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <tf/tf.h>
+
+#include <std_srvs/Empty.h>
+
+// Floating-point value comparison threshold
+const double EPS = 0.01;
+const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
+const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
+const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
+const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
+const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
+
+class DiffDriveControllerTest : public ::testing::Test
+{
+public:
+
+ DiffDriveControllerTest()
+ : received_first_odom(false)
+ , cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
+ , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this))
+ , vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this))
+ , joint_traj_controller_state_sub(nh.subscribe("wheel_joint_controller_state", 100, &DiffDriveControllerTest::jointTrajectoryControllerStateCallback, this))
+ , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
+ , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
+ {
+ }
+
+ ~DiffDriveControllerTest()
+ {
+ odom_sub.shutdown();
+ joint_traj_controller_state_sub.shutdown();
+ }
+
+ nav_msgs::Odometry getLastOdom(){ return last_odom; }
+ geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; }
+ control_msgs::JointTrajectoryControllerState getLastJointTrajectoryControllerState(){ return last_joint_traj_controller_state; }
+ void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
+ bool isControllerAlive()const{ return (odom_sub.getNumPublishers() > 0); }
+ bool isPublishingCmdVelOut(const ros::Duration &timeout=ros::Duration(1)) const
+ {
+ ros::Time start = ros::Time::now();
+ int get_num_publishers = vel_out_sub.getNumPublishers();
+ while ( (get_num_publishers == 0) && (ros::Time::now() < start + timeout) ) {
+ ros::Duration(0.1).sleep();
+ get_num_publishers = vel_out_sub.getNumPublishers();
+ }
+ return (get_num_publishers > 0);
+ }
+ bool isPublishingJointTrajectoryControllerState(){ return (joint_traj_controller_state_sub.getNumPublishers() > 0); }
+ bool hasReceivedFirstOdom()const{ return received_first_odom; }
+
+ void start(){ std_srvs::Empty srv; start_srv.call(srv); }
+ void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
+
+ void waitForController() const
+ {
+ while(!isControllerAlive() && ros::ok())
+ {
+ ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for controller.");
+ ros::Duration(0.1).sleep();
+ }
+ if (!ros::ok())
+ FAIL() << "Something went wrong while executing test.";
+ }
+
+ void waitForOdomMsgs() const
+ {
+ while(!hasReceivedFirstOdom() && ros::ok())
+ {
+ ROS_DEBUG_STREAM_THROTTLE(0.5, "Waiting for odom messages to be published.");
+ ros::Duration(0.01).sleep();
+ }
+ if (!ros::ok())
+ FAIL() << "Something went wrong while executing test.";
+ }
+
+private:
+ bool received_first_odom;
+ ros::NodeHandle nh;
+ ros::Publisher cmd_pub;
+ ros::Subscriber odom_sub;
+ ros::Subscriber vel_out_sub;
+ nav_msgs::Odometry last_odom;
+ geometry_msgs::TwistStamped last_cmd_vel_out;
+ ros::Subscriber joint_traj_controller_state_sub;
+ control_msgs::JointTrajectoryControllerState last_joint_traj_controller_state;
+
+ ros::ServiceClient start_srv;
+ ros::ServiceClient stop_srv;
+
+ void odomCallback(const nav_msgs::Odometry& odom)
+ {
+ ROS_INFO_STREAM("Callback received: pos.x: " << odom.pose.pose.position.x
+ << ", orient.z: " << odom.pose.pose.orientation.z
+ << ", lin_est: " << odom.twist.twist.linear.x
+ << ", ang_est: " << odom.twist.twist.angular.z);
+ last_odom = odom;
+ received_first_odom = true;
+ }
+
+ void jointTrajectoryControllerStateCallback(const control_msgs::JointTrajectoryControllerState& joint_traj_controller_state)
+ {
+ ROS_INFO_STREAM("Joint trajectory controller state callback.");
+ ROS_DEBUG_STREAM("Joint trajectory controller state callback received:\n" <<
+ joint_traj_controller_state);
+
+ last_joint_traj_controller_state = joint_traj_controller_state;
+ }
+
+ void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out)
+ {
+ ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x
+ << ", ang: " << cmd_vel_out.twist.angular.z);
+ last_cmd_vel_out = cmd_vel_out;
+ }
+};
+
+inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
+{
+ return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
+}