HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
mir_robot/diff_drive_controller/test/diff_drive_bad_urdf.test Executable file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with bad urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_bad.xacro'" />
<!-- Controller test -->
<test test-name="diff_drive_fail_test"
pkg="diff_drive_controller"
type="diff_drive_fail_test"
time-limit="10.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

35
mir_robot/diff_drive_controller/test/diff_drive_common.launch Executable file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<launch>
<!-- Use simulation time -->
<rosparam param="use_sim_time">True</rosparam>
<!-- Load diffbot model -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
<!-- Start diffbot -->
<node name="diffbot"
pkg="diff_drive_controller"
type="diffbot"/>
<!-- Load controller config -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.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>

14
mir_robot/diff_drive_controller/test/diff_drive_controller.test Executable file
View File

@@ -0,0 +1,14 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Controller test -->
<test test-name="diff_drive_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

17
mir_robot/diff_drive_controller/test/diff_drive_controller_limits.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Load diff-drive limits -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_limits.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_limits_test"
pkg="diff_drive_controller"
type="diff_drive_limits_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

20
mir_robot/diff_drive_controller/test/diff_drive_controller_nan.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<rosparam>
diffbot_controller:
publish_cmd: True
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_nan_test"
pkg="diff_drive_controller"
type="diff_drive_nan_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
</test>
</launch>

21
mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out.test Executable file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- default value -->
<!-- <rosparam>
diffbot_controller:
publish_cmd: False
</rosparam> -->
<!-- Controller test -->
<test test-name="diff_drive_default_cmd_vel_out_test"
pkg="diff_drive_controller"
type="diff_drive_default_cmd_vel_out_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
</test>
</launch>

69
mir_robot/diff_drive_controller/test/diff_drive_default_cmd_vel_out_test.cpp Executable file
View File

@@ -0,0 +1,69 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2017, PAL Robotics.
//
// 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 Jeremie Deray
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testDefaultCmdVelOutTopic)
{
// wait for ROS
waitForController();
// msgs are published in the same loop
// thus if odom is published cmd_vel_out
// should be as well (if enabled)
waitForOdomMsgs();
EXPECT_FALSE(isPublishingCmdVelOut());
// 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(0.1).sleep();
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
ros::Duration(0.1).sleep();
EXPECT_FALSE(isPublishingCmdVelOut());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_default_cmd_vel_out_topic_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

20
mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Do not specific an odom_frame_id -->
<!-- <rosparam>
diffbot_controller:
odom_frame_id: odom
</rosparam> -->
<!-- Controller test -->
<test test-name="diff_drive_default_odom_frame_test"
pkg="diff_drive_controller"
type="diff_drive_default_odom_frame_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

69
mir_robot/diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp Executable file
View File

@@ -0,0 +1,69 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015, Locus Robotics Corp.
//
// 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 Eric Tappan
#include "test_common.h"
#include <tf/transform_listener.h>
// TEST CASES
TEST_F(DiffDriveControllerTest, testOdomFrame)
{
// wait for ROS
waitForController();
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the original odom frame doesn't exist
EXPECT_TRUE(listener.frameExists("odom"));
}
TEST_F(DiffDriveControllerTest, testOdomTopic)
{
// wait for ROS
waitForController();
waitForOdomMsgs();
// get an odom message
nav_msgs::Odometry odom_msg = getLastOdom();
// check its frame_id
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom");
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_default_odom_frame_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

21
mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state.test Executable file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- default value -->
<!-- <rosparam>
diffbot_controller:
publish_wheel_joint_controller_state: False
</rosparam> -->
<!-- Controller test -->
<test test-name="diff_drive_default_wheel_joint_controller_state_test"
pkg="diff_drive_controller"
type="diff_drive_default_wheel_joint_controller_state_test"
time-limit="30.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
<remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
</test>
</launch>

65
mir_robot/diff_drive_controller/test/diff_drive_default_wheel_joint_controller_state_test.cpp Executable file
View File

@@ -0,0 +1,65 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2018, PAL Robotics.
//
// 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 Jeremie Deray
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testDefaultJointTrajectoryControllerStateTopic)
{
// wait for ROS
waitForController();
EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
// 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(0.1).sleep();
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
ros::Duration(0.1).sleep();
EXPECT_FALSE(isPublishingJointTrajectoryControllerState());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_default_wheel_joint_controller_state_topic_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

20
mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Make sure to disable odom tf -->
<rosparam>
diffbot_controller:
enable_odom_tf: False
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_dyn_reconf_test"
pkg="diff_drive_controller"
type="diff_drive_dyn_reconf_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

176
mir_robot/diff_drive_controller/test/diff_drive_dyn_reconf_test.cpp Executable file
View File

@@ -0,0 +1,176 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2018, 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 Jeremie Deray
#include "test_common.h"
#include <tf/transform_listener.h>
#include <dynamic_reconfigure/server.h>
// TEST CASES
TEST_F(DiffDriveControllerTest, testDynReconfServerAlive)
{
// wait for ROS
waitForController();
// Expect server is alive
EXPECT_TRUE(ros::service::exists("diffbot_controller/set_parameters", true));
dynamic_reconfigure::ReconfigureRequest srv_req;
dynamic_reconfigure::ReconfigureResponse srv_resp;
// Expect server is callable (get-fashion)
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
EXPECT_EQ(1, srv_resp.config.bools.size());
if (!srv_resp.config.bools.empty())
{
EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
// expect false since it is set to false in the .test
EXPECT_EQ(false, srv_resp.config.bools[0].value);
}
EXPECT_EQ(4, srv_resp.config.doubles.size());
if (srv_resp.config.doubles.size() >= 4)
{
EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
EXPECT_EQ(1, srv_resp.config.doubles[0].value);
EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
EXPECT_EQ(1, srv_resp.config.doubles[1].value);
EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
EXPECT_EQ(1, srv_resp.config.doubles[2].value);
EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
EXPECT_EQ(50, srv_resp.config.doubles[3].value);
}
dynamic_reconfigure::DoubleParameter double_param;
double_param.name = "left_wheel_radius_multiplier";
double_param.value = 0.95;
srv_req.config.doubles.push_back(double_param);
double_param.name = "right_wheel_radius_multiplier";
double_param.value = 0.95;
srv_req.config.doubles.push_back(double_param);
double_param.name = "wheel_separation_multiplier";
double_param.value = 0.95;
srv_req.config.doubles.push_back(double_param);
double_param.name = "publish_rate";
double_param.value = 150;
srv_req.config.doubles.push_back(double_param);
dynamic_reconfigure::BoolParameter bool_param;
bool_param.name = "enable_odom_tf";
bool_param.value = false;
srv_req.config.bools.push_back(bool_param);
// Expect server is callable (set-fashion)
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
EXPECT_EQ(1, srv_resp.config.bools.size());
if (!srv_resp.config.bools.empty())
{
EXPECT_EQ("enable_odom_tf", srv_resp.config.bools[0].name);
EXPECT_EQ(false, srv_resp.config.bools[0].value);
}
EXPECT_EQ(4, srv_resp.config.doubles.size());
if (srv_resp.config.doubles.size() >= 4)
{
EXPECT_EQ("left_wheel_radius_multiplier", srv_resp.config.doubles[0].name);
EXPECT_EQ(0.95, srv_resp.config.doubles[0].value);
EXPECT_EQ("right_wheel_radius_multiplier", srv_resp.config.doubles[1].name);
EXPECT_EQ(0.95, srv_resp.config.doubles[1].value);
EXPECT_EQ("wheel_separation_multiplier", srv_resp.config.doubles[2].name);
EXPECT_EQ(0.95, srv_resp.config.doubles[2].value);
EXPECT_EQ("publish_rate", srv_resp.config.doubles[3].name);
EXPECT_EQ(150, srv_resp.config.doubles[3].value);
}
}
// TEST CASES
TEST_F(DiffDriveControllerTest, testDynReconfEnableTf)
{
// wait for ROS
while(!isControllerAlive() && ros::ok())
{
ros::Duration(0.1).sleep();
}
if (!ros::ok())
FAIL() << "Something went wrong while executing test";
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the odom frame doesn't exist
EXPECT_FALSE(listener.frameExists("odom"));
dynamic_reconfigure::ReconfigureRequest srv_req;
dynamic_reconfigure::ReconfigureResponse srv_resp;
dynamic_reconfigure::BoolParameter bool_param;
bool_param.name = "enable_odom_tf";
bool_param.value = true;
srv_req.config.bools.push_back(bool_param);
EXPECT_TRUE(ros::service::call("diffbot_controller/set_parameters", srv_req, srv_resp));
ros::Duration(2.0).sleep();
// check the odom frame doesn't exist
EXPECT_TRUE(listener.frameExists("odom"));
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_dyn_reconf_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

60
mir_robot/diff_drive_controller/test/diff_drive_fail_test.cpp Executable file
View File

@@ -0,0 +1,60 @@
///////////////////////////////////////////////////////////////////////////////
// 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 Paul Mathieu
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testWrongJointName)
{
// the controller should never be alive
int secs = 0;
while(!isControllerAlive() && ros::ok() && secs < 5)
{
ros::Duration(1.0).sleep();
secs++;
}
if (!ros::ok())
FAIL() << "Something went wrong while executing test.";
// give up and assume controller load failure after 5 seconds
EXPECT_GE(secs, 5);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_fail_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

19
mir_robot/diff_drive_controller/test/diff_drive_get_wheel_radius_fail.test Executable file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override with wrong controller configuration -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
<param name="diffbot_controller/wheel_separation" value="0.08"/>
<!-- Controller test -->
<test test-name="diff_drive_fail_test"
pkg="diff_drive_controller"
type="diff_drive_fail_test"
time-limit="10.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

17
mir_robot/diff_drive_controller/test/diff_drive_left_right_multipliers.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Load diff drive parameter multipliers -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_left_right_multipliers.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

231
mir_robot/diff_drive_controller/test/diff_drive_limits_test.cpp Executable file
View File

@@ -0,0 +1,231 @@
///////////////////////////////////////////////////////////////////////////////
// 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 Paul Mathieu
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testLinearJerkLimits)
{
// 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 big command
cmd_vel.linear.x = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(0.5).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 0.37m.s-1
EXPECT_NEAR(new_odom.twist.twist.linear.x, 0.37, JERK_LINEAR_VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
cmd_vel.linear.x = 0.0;
publish(cmd_vel);
}
TEST_F(DiffDriveControllerTest, testLinearAccelerationLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// 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 big command
cmd_vel.linear.x = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(0.5).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 0.5 m.s-1, which is 1.0m.s-2 * 0.5s
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 0.5 + VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
cmd_vel.linear.x = 0.0;
publish(cmd_vel);
}
TEST_F(DiffDriveControllerTest, testLinearVelocityLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// 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 big command
cmd_vel.linear.x = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(5.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 1.0 m.s-1, the limit
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), 1.0 + VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), EPS);
cmd_vel.linear.x = 0.0;
publish(cmd_vel);
}
/* This test has been failing on Travis for a long time due to timing issues however it works well when ran manually
TEST_F(DiffDriveControllerTest, testAngularJerkLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// 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 big command
cmd_vel.angular.z = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(0.5).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 0.7rad.s-1
EXPECT_NEAR(new_odom.twist.twist.angular.z, 0.7, JERK_ANGULAR_VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
}
*/
TEST_F(DiffDriveControllerTest, testAngularAccelerationLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// 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 big command
cmd_vel.angular.z = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(0.5).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 1.0rad.s-1, which is 2.0rad.s-2 * 0.5s
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 1.0 + VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
}
TEST_F(DiffDriveControllerTest, testAngularVelocityLimits)
{
// wait for ROS
while(!isControllerAlive())
{
ros::Duration(0.1).sleep();
}
// 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 big command
cmd_vel.angular.z = 10.0;
publish(cmd_vel);
// wait for a while
ros::Duration(5.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot speed is now 2.0rad.s-1, the limit
EXPECT_LT(fabs(new_odom.twist.twist.angular.z - old_odom.twist.twist.angular.z), 2.0 + VELOCITY_TOLERANCE);
EXPECT_LT(fabs(new_odom.twist.twist.linear.x - old_odom.twist.twist.linear.x), EPS);
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_limits_test");
ros::AsyncSpinner spinner(1);
spinner.start();
//ros::Duration(0.5).sleep();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

24
mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers.test Executable file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with sphere wheel urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
<!-- Set allow_multiple_publishers to false -->
<param name="diffbot_controller/allow_multiple_cmd_vel_publishers" value="false"/>
<node name="cmd_vel_publisher" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}}'" />
<node name="cmd_vel_publisher2" pkg="rostopic" type="rostopic" args="pub -r 2 /diffbot_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 2.0}}'" />
<!-- Controller test -->
<test test-name="diff_drive_allow_multiple_cmd_vel_publishers_param_test"
pkg="diff_drive_controller"
type="diff_drive_multiple_cmd_vel_publishers_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

72
mir_robot/diff_drive_controller/test/diff_drive_multiple_cmd_vel_publishers_test.cpp Executable file
View File

@@ -0,0 +1,72 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2014, 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"
// TEST CASES
TEST_F(DiffDriveControllerTest, breakWithMultiplePublishers)
{
// wait for ROS
waitForController();
waitForOdomMsgs();
nav_msgs::Odometry old_odom = getLastOdom();
//TODO: we should be programatically publish from 2 different nodes
// not the current hacky solution with the launch files
ros::Duration(1.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
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), 0.0, 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);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_multiple_publishers_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

17
mir_robot/diff_drive_controller/test/diff_drive_multipliers.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Load diff drive parameter multipliers -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_multipliers.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

131
mir_robot/diff_drive_controller/test/diff_drive_nan_test.cpp Executable file
View File

@@ -0,0 +1,131 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2014, 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 Enrique Fernandez
#include "test_common.h"
// NaN
#include <limits>
// TEST CASES
TEST_F(DiffDriveControllerTest, testNaN) {
// 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();
// send a command
cmd_vel.linear.x = 0.1;
ros::Duration(2.0).sleep();
// stop robot (will generate NaN)
stop();
ros::Duration(2.0).sleep();
nav_msgs::Odometry odom = getLastOdom();
EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x));
EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
// start robot
start();
ros::Duration(2.0).sleep();
odom = getLastOdom();
EXPECT_FALSE(std::isnan(odom.twist.twist.linear.x));
EXPECT_FALSE(std::isnan(odom.twist.twist.angular.z));
EXPECT_FALSE(std::isnan(odom.pose.pose.position.x));
EXPECT_FALSE(std::isnan(odom.pose.pose.position.y));
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.z));
EXPECT_FALSE(std::isnan(odom.pose.pose.orientation.w));
}
TEST_F(DiffDriveControllerTest, testNaNCmd) {
// wait for ROS
while (!isControllerAlive()) {
ros::Duration(0.1).sleep();
}
// 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(0.1).sleep();
// send NaN
for (int i = 0; i < 10; ++i) {
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = NAN;
cmd_vel.angular.z = 0.0;
publish(cmd_vel);
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
ros::Duration(0.1).sleep();
}
for (int i = 0; i < 10; ++i) {
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0.0;
cmd_vel.angular.z = NAN;
publish(cmd_vel);
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
EXPECT_FALSE(std::isnan(odom_msg.twist.linear.x));
EXPECT_FALSE(std::isnan(odom_msg.twist.angular.z));
ros::Duration(0.1).sleep();
}
nav_msgs::Odometry odom = getLastOdom();
EXPECT_DOUBLE_EQ(odom.twist.twist.linear.x, 0.0);
EXPECT_DOUBLE_EQ(odom.pose.pose.position.x, 0.0);
EXPECT_DOUBLE_EQ(odom.pose.pose.position.y, 0.0);
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
EXPECT_DOUBLE_EQ(odom_msg.twist.linear.x, 0.0);
}
int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_nan_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

20
mir_robot/diff_drive_controller/test/diff_drive_odom_frame.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Set odom_frame_id to something new -->
<rosparam>
diffbot_controller:
odom_frame_id: new_odom
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_odom_frame_test"
pkg="diff_drive_controller"
type="diff_drive_odom_frame_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

83
mir_robot/diff_drive_controller/test/diff_drive_odom_frame_test.cpp Executable file
View File

@@ -0,0 +1,83 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015, Locus Robotics Corp.
//
// 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 Eric Tappan
#include "test_common.h"
#include <tf/transform_listener.h>
// TEST CASES
TEST_F(DiffDriveControllerTest, testNoOdomFrame)
{
// wait for ROS
waitForController();
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the original odom frame doesn't exist
EXPECT_FALSE(listener.frameExists("odom"));
}
TEST_F(DiffDriveControllerTest, testNewOdomFrame)
{
// wait for ROS
waitForController();
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the new_odom frame does exist
EXPECT_TRUE(listener.frameExists("new_odom"));
}
TEST_F(DiffDriveControllerTest, testOdomTopic)
{
// wait for ROS
waitForController();
// wait until we received first odom msg
waitForOdomMsgs();
// get an odom message
nav_msgs::Odometry odom_msg = getLastOdom();
// check its frame_id
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom");
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_odom_frame_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

20
mir_robot/diff_drive_controller/test/diff_drive_odom_tf.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Make sure to disable odom tf -->
<rosparam>
diffbot_controller:
enable_odom_tf: False
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_odom_tf_test"
pkg="diff_drive_controller"
type="diff_drive_odom_tf_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

57
mir_robot/diff_drive_controller/test/diff_drive_odom_tf_test.cpp Executable file
View File

@@ -0,0 +1,57 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2014, 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, testNoOdomFrame)
{
// wait for ROS
waitForController();
// set up tf listener
tf::TransformListener listener;
ros::Duration(2.0).sleep();
// check the odom frame doesn't exist
EXPECT_FALSE(listener.frameExists("odom"));
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_odom_tf_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

17
mir_robot/diff_drive_controller/test/diff_drive_open_loop.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Load diff drive parameter open loop -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_open_loop.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

20
mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<rosparam>
diffbot_controller:
publish_cmd: True
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_pub_cmd_vel_out_test"
pkg="diff_drive_controller"
type="diff_drive_pub_cmd_vel_out_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
<remap from="cmd_vel_out" to="diffbot_controller/cmd_vel_out" />
</test>
</launch>

74
mir_robot/diff_drive_controller/test/diff_drive_pub_cmd_vel_out_test.cpp Executable file
View File

@@ -0,0 +1,74 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2017, PAL Robotics.
//
// 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 Jeremie Deray
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testCmdVelOutTopic)
{
// wait for ROS
waitForController();
// msgs are published in the same loop
// thus if odom is published cmd_vel_out
// should be as well (if enabled)
waitForOdomMsgs();
EXPECT_TRUE(isPublishingCmdVelOut());
// 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(0.1).sleep();
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
ros::Duration(0.1).sleep();
EXPECT_TRUE(isPublishingCmdVelOut());
// get a twist message
geometry_msgs::TwistStamped odom_msg = getLastCmdVelOut();
EXPECT_GT(fabs(odom_msg.twist.linear.x), 0);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_pub_cmd_vel_out_topic_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

21
mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state.test Executable file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- default value -->
<rosparam>
diffbot_controller:
publish_wheel_joint_controller_state: True
</rosparam>
<!-- Controller test -->
<test test-name="diff_drive_publish_wheel_joint_controller_state__test"
pkg="diff_drive_controller"
type="diff_drive_publish_wheel_joint_controller_state_test"
time-limit="30.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
<remap from="wheel_joint_controller_state" to="diffbot_controller/wheel_joint_controller_state" />
</test>
</launch>

65
mir_robot/diff_drive_controller/test/diff_drive_publish_wheel_joint_controller_state_test.cpp Executable file
View File

@@ -0,0 +1,65 @@
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2018, PAL Robotics.
//
// 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 Jeremie Deray
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testPublishJointTrajectoryControllerStateTopic)
{
// wait for ROS
waitForController();
EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
// 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(0.1).sleep();
cmd_vel.linear.x = 0.1;
publish(cmd_vel);
ros::Duration(0.1).sleep();
EXPECT_TRUE(isPublishingJointTrajectoryControllerState());
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_publish_wheel_joint_controller_state_topic_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

20
mir_robot/diff_drive_controller/test/diff_drive_radius_param.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with square wheel urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
<!-- Provide the radius, since the bot's wheels are boxes, not cylinders or spheres -->
<param name="diffbot_controller/wheel_radius" value="0.11"/>
<!-- Controller test -->
<test test-name="diff_drive_wheel_radius_param_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

20
mir_robot/diff_drive_controller/test/diff_drive_radius_param_fail.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with bad urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_square_wheels.xacro'" />
<!-- Don't provide the radius parameter, so the controller should break -->
<!-- <param name="diffbot_controller/wheel_radius" value="0.11"/> -->
<!-- Controller test -->
<test test-name="diff_drive_wheel_radius_param_fail_test"
pkg="diff_drive_controller"
type="diff_drive_fail_test"
time-limit="10.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

20
mir_robot/diff_drive_controller/test/diff_drive_radius_sphere.test Executable file
View File

@@ -0,0 +1,20 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with sphere wheel urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
<!-- Don't provide the radius, since controller should accept cylinders *or* spheres -->
<!-- param name="diffbot_controller/wheel_radius" value="0.11"/ -->
<!-- Controller test -->
<test test-name="diff_drive_wheel_radius_param_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

22
mir_robot/diff_drive_controller/test/diff_drive_separation_param.test Executable file
View File

@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override robot_description with sphere wheel urdf -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot_sphere_wheels.xacro'" />
<!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
<param name="diffbot_controller/wheel_radius" value="0.11"/>
<!-- Provide the wheel separation -->
<param name="diffbot_controller/wheel_separation" value="1.0"/>
<!-- Controller test -->
<test test-name="diff_drive_wheel_separation_param_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

154
mir_robot/diff_drive_controller/test/diff_drive_test.cpp Executable file
View File

@@ -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;
}

17
mir_robot/diff_drive_controller/test/diff_drive_timeout.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Load diff-drive cmd_vel_timeout -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_timeout.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_timeout_test"
pkg="diff_drive_controller"
type="diff_drive_timeout_test"
time-limit="20.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

70
mir_robot/diff_drive_controller/test/diff_drive_timeout_test.cpp Executable file
View File

@@ -0,0 +1,70 @@
///////////////////////////////////////////////////////////////////////////////
// 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 Paul Mathieu
#include "test_common.h"
// TEST CASES
TEST_F(DiffDriveControllerTest, testTimeout)
{
// 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);
// give some time to the controller to react to the command
ros::Duration(0.1).sleep();
// get initial odom
nav_msgs::Odometry old_odom = getLastOdom();
// send a velocity command of 1 m/s
cmd_vel.linear.x = 1.0;
publish(cmd_vel);
// wait a bit
ros::Duration(3.0).sleep();
nav_msgs::Odometry new_odom = getLastOdom();
// check if the robot has stopped after 0.5s, thus covering less than 0.5s*1.0m.s-1 + some (big) tolerance
EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), 0.8);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "diff_drive_test");
ros::AsyncSpinner spinner(1);
spinner.start();
int ret = RUN_ALL_TESTS();
spinner.stop();
ros::shutdown();
return ret;
}

17
mir_robot/diff_drive_controller/test/diff_drive_wrong.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
<!-- Override with wrong controller configuration -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_wrong.yaml" />
<!-- Controller test -->
<test test-name="diff_drive_fail_test"
pkg="diff_drive_controller"
type="diff_drive_fail_test"
time-limit="10.0">
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
<remap from="odom" to="diffbot_controller/odom" />
</test>
</launch>

93
mir_robot/diff_drive_controller/test/diffbot.cpp Executable file
View File

@@ -0,0 +1,93 @@
///////////////////////////////////////////////////////////////////////////////
// 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.
//////////////////////////////////////////////////////////////////////////////
// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
#include "diffbot.h"
#include <chrono>
#include <thread>
#include <controller_manager/controller_manager.h>
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "diffbot");
ros::NodeHandle nh;
// This should be set in launch files as well
nh.setParam("/use_sim_time", true);
Diffbot<> robot;
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
controller_manager::ControllerManager cm(&robot, nh);
ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
ros::AsyncSpinner spinner(1);
spinner.start();
std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
ros::Time internal_time(0);
const ros::Duration dt = robot.getPeriod();
double elapsed_secs = 0;
while(ros::ok())
{
begin = std::chrono::system_clock::now();
robot.read();
cm.update(internal_time, dt);
robot.write();
end = std::chrono::system_clock::now();
elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
if (dt.toSec() - elapsed_secs < 0.0)
{
ROS_WARN_STREAM_THROTTLE(
0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
}
else
{
ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
}
rosgraph_msgs::Clock clock;
clock.clock = ros::Time(internal_time);
clock_publisher.publish(clock);
internal_time += dt;
}
spinner.stop();
return 0;
}

143
mir_robot/diff_drive_controller/test/diffbot.h Executable file
View File

@@ -0,0 +1,143 @@
///////////////////////////////////////////////////////////////////////////////
// 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.
//////////////////////////////////////////////////////////////////////////////
// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
#pragma once
// ROS
#include <ros/ros.h>
#include <std_srvs/Empty.h>
// ros_control
#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <realtime_tools/realtime_buffer.h>
// NaN
#include <limits>
// ostringstream
#include <sstream>
template <unsigned int NUM_JOINTS = 2>
class Diffbot : public hardware_interface::RobotHW
{
public:
Diffbot()
: running_(true)
, start_srv_(nh_.advertiseService("start", &Diffbot::start_callback, this))
, stop_srv_(nh_.advertiseService("stop", &Diffbot::stop_callback, this))
{
// Intialize raw data
std::fill_n(pos_, NUM_JOINTS, 0.0);
std::fill_n(vel_, NUM_JOINTS, 0.0);
std::fill_n(eff_, NUM_JOINTS, 0.0);
std::fill_n(cmd_, NUM_JOINTS, 0.0);
// Connect and register the joint state and velocity interface
for (unsigned int i = 0; i < NUM_JOINTS; ++i)
{
std::ostringstream os;
os << "wheel_" << i << "_joint";
hardware_interface::JointStateHandle state_handle(os.str(), &pos_[i], &vel_[i], &eff_[i]);
jnt_state_interface_.registerHandle(state_handle);
hardware_interface::JointHandle vel_handle(jnt_state_interface_.getHandle(os.str()), &cmd_[i]);
jnt_vel_interface_.registerHandle(vel_handle);
}
registerInterface(&jnt_state_interface_);
registerInterface(&jnt_vel_interface_);
}
ros::Time getTime() const {return ros::Time::now();}
ros::Duration getPeriod() const {return ros::Duration(0.01);}
void read()
{
// Read the joint state of the robot into the hardware interface
if (running_)
{
for (unsigned int i = 0; i < NUM_JOINTS; ++i)
{
// Note that pos_[i] will be NaN for one more cycle after we start(),
// but that is consistent with the knowledge we have about the state
// of the robot.
pos_[i] += vel_[i]*getPeriod().toSec(); // update position
vel_[i] = cmd_[i]; // might add smoothing here later
}
}
else
{
std::fill_n(pos_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
std::fill_n(vel_, NUM_JOINTS, std::numeric_limits<double>::quiet_NaN());
}
}
void write()
{
// Write the commands to the joints
std::ostringstream os;
for (unsigned int i = 0; i < NUM_JOINTS - 1; ++i)
{
os << cmd_[i] << ", ";
}
os << cmd_[NUM_JOINTS - 1];
ROS_INFO_STREAM("Commands for joints: " << os.str());
}
bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
running_ = true;
return true;
}
bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
running_ = false;
return true;
}
private:
hardware_interface::JointStateInterface jnt_state_interface_;
hardware_interface::VelocityJointInterface jnt_vel_interface_;
double cmd_[NUM_JOINTS];
double pos_[NUM_JOINTS];
double vel_[NUM_JOINTS];
double eff_[NUM_JOINTS];
bool running_;
ros::NodeHandle nh_;
ros::ServiceServer start_srv_;
ros::ServiceServer stop_srv_;
};

77
mir_robot/diff_drive_controller/test/diffbot.xacro Executable file
View File

@@ -0,0 +1,77 @@
<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
-->
<xacro:include filename="$(find diff_drive_controller)/test/wheel.xacro"/>
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<!-- Constants for robot dimensions -->
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<!-- Links: inertial,visual,collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<!-- Wheels -->
<xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
</robot>

142
mir_robot/diff_drive_controller/test/diffbot_bad.xacro Executable file
View File

@@ -0,0 +1,142 @@
<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
-->
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<!-- Links: inertial,visual,collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<link name="wheel1">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</collision>
</link>
<joint name="joint_w1" type="continuous">
<parent link="base_link"/>
<child link="wheel1"/>
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="wheel2">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 ${PI/2} 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</collision>
</link>
<joint name="joint_w2" type="continuous">
<parent link="base_link"/>
<child link="wheel2"/>
<origin xyz="${-width/2-axel_offset} 0 0" rpy="${-PI/2} 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="joint_w1_trans" type="SimpleTransmission">
<actuator name="joint_w1_motor" />
<joint name="joint_w1" />
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<transmission name="joint_w2_trans" type="SimpleTransmission">
<actuator name="joint_w2_motor" />
<joint name="joint_w2" />
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="wheel1">
<material>Gazebo/Red</material>
</gazebo>
<gazebo reference="wheel2">
<material>Gazebo/Blue</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
</robot>

8
mir_robot/diff_drive_controller/test/diffbot_controllers.yaml Executable file
View File

@@ -0,0 +1,8 @@
diffbot_controller:
type: "diff_drive_controller/DiffDriveController"
left_wheel: 'wheel_0_joint'
right_wheel: 'wheel_1_joint'
publish_rate: 50.0 # defaults to 50
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

4
mir_robot/diff_drive_controller/test/diffbot_left_right_multipliers.yaml Executable file
View File

@@ -0,0 +1,4 @@
diffbot_controller:
wheel_separation_multiplier: 2.3
left_wheel_radius_multiplier: 1.4
right_wheel_radius_multiplier: 1.4

19
mir_robot/diff_drive_controller/test/diffbot_limits.yaml Executable file
View File

@@ -0,0 +1,19 @@
diffbot_controller:
linear:
x:
has_velocity_limits: true
min_velocity: -0.5
max_velocity: 1.0
has_acceleration_limits: true
min_acceleration: -0.5
max_acceleration: 1.0
has_jerk_limits: true
max_jerk: 5.0
angular:
z:
has_velocity_limits: true
max_velocity: 2.0
has_acceleration_limits: true
max_acceleration: 2.0
has_jerk_limits: true
max_jerk: 10.0

3
mir_robot/diff_drive_controller/test/diffbot_multipliers.yaml Executable file
View File

@@ -0,0 +1,3 @@
diffbot_controller:
wheel_separation_multiplier: 2.3
wheel_radius_multiplier: 1.4

2
mir_robot/diff_drive_controller/test/diffbot_open_loop.yaml Executable file
View File

@@ -0,0 +1,2 @@
diffbot_controller:
open_loop: true

75
mir_robot/diff_drive_controller/test/diffbot_sphere_wheels.xacro Executable file
View File

@@ -0,0 +1,75 @@
<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
-->
<xacro:include filename="$(find diff_drive_controller)/test/sphere_wheel.xacro"/>
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<!-- Constants for robot dimensions -->
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<!-- Links: inertial,visual,collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<!-- Wheels -->
<xacro:sphere_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:sphere_wheel>
<xacro:sphere_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:sphere_wheel>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
</robot>

75
mir_robot/diff_drive_controller/test/diffbot_square_wheels.xacro Executable file
View File

@@ -0,0 +1,75 @@
<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
-->
<xacro:include filename="$(find diff_drive_controller)/test/square_wheel.xacro"/>
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<!-- Constants for robot dimensions -->
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<!-- Links: inertial,visual,collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<!-- Wheels -->
<xacro:square_wheel name="wheel_0" parent="base" radius="${wheel_radius}">
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:square_wheel>
<xacro:square_wheel name="wheel_1" parent="base" radius="${wheel_radius}">
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:square_wheel>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
</robot>

2
mir_robot/diff_drive_controller/test/diffbot_timeout.yaml Executable file
View File

@@ -0,0 +1,2 @@
diffbot_controller:
cmd_vel_timeout: 0.5

8
mir_robot/diff_drive_controller/test/diffbot_wrong.yaml Executable file
View File

@@ -0,0 +1,8 @@
diffbot_controller:
type: "diff_drive_controller/DiffDriveController"
left_wheel: 'this_joint_does_not_exist'
right_wheel: 'right_wheel_joint'
publish_rate: 50.0 # defaults to 50
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

35
mir_robot/diff_drive_controller/test/skid_steer_common.launch Executable file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<launch>
<!-- Use simulation time -->
<rosparam param="use_sim_time">True</rosparam>
<!-- Load skidsteerbot model -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
<!-- Start skidsteerbot -->
<node name="skidsteerbot"
pkg="diff_drive_controller"
type="skidsteerbot"/>
<!-- Load controller config -->
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
<!-- Spawn controller -->
<node name="controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="skidsteerbot_controller" />
<!-- rqt_plot monitoring -->
<!--
<node name="skidsteerbot_pos_monitor"
pkg="rqt_plot"
type="rqt_plot"
args="/skidsteerbot_controller/odom/pose/pose/position/x" />
<node name="skidsteerbot_vel_monitor"
pkg="rqt_plot"
type="rqt_plot"
args="/skidsteerbot_controller/odom/twist/twist/linear/x" />
-->
</launch>

14
mir_robot/diff_drive_controller/test/skid_steer_controller.test Executable file
View File

@@ -0,0 +1,14 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
<!-- Controller test -->
<test test-name="skid_steer_test"
pkg="diff_drive_controller"
type="diff_drive_test"
time-limit="80.0">
<remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
<remap from="odom" to="skidsteerbot_controller/odom" />
</test>
</launch>

17
mir_robot/diff_drive_controller/test/skid_steer_no_wheels.test Executable file
View File

@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load common test stuff -->
<include file="$(find diff_drive_controller)/test/skid_steer_common.launch" />
<!-- Override with wrong controller configuration -->
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_no_wheels.yaml" />
<!-- Controller test -->
<test test-name="skid_steer_fail_test"
pkg="diff_drive_controller"
type="diff_drive_fail_test"
time-limit="10.0">
<remap from="cmd_vel" to="skidsteerbot_controller/cmd_vel" />
<remap from="odom" to="skidsteerbot_controller/odom" />
</test>
</launch>

93
mir_robot/diff_drive_controller/test/skidsteerbot.cpp Executable file
View File

@@ -0,0 +1,93 @@
///////////////////////////////////////////////////////////////////////////////
// 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.
//////////////////////////////////////////////////////////////////////////////
// NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
#include "diffbot.h"
#include <chrono>
#include <thread>
#include <controller_manager/controller_manager.h>
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "skidsteerbot");
ros::NodeHandle nh;
// This should be set in launch files as well
nh.setParam("/use_sim_time", true);
Diffbot<6> robot;
ROS_WARN_STREAM("period: " << robot.getPeriod().toSec());
controller_manager::ControllerManager cm(&robot, nh);
ros::Publisher clock_publisher = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
//ros::Rate rate(1.0 / robot.getPeriod().toSec());
ros::AsyncSpinner spinner(1);
spinner.start();
std::chrono::system_clock::time_point begin = std::chrono::system_clock::now();
std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
ros::Time internal_time(0);
const ros::Duration dt = robot.getPeriod();
double elapsed_secs = 0;
while(ros::ok())
{
begin = std::chrono::system_clock::now();
robot.read();
cm.update(internal_time, dt);
robot.write();
end = std::chrono::system_clock::now();
elapsed_secs = std::chrono::duration_cast<std::chrono::duration<double> >((end - begin)).count();
if (dt.toSec() - elapsed_secs < 0.0)
{
ROS_WARN_STREAM_THROTTLE(
0.1, "Control cycle is taking to much time, elapsed: " << elapsed_secs);
}
else
{
ROS_DEBUG_STREAM_THROTTLE(1.0, "Control cycle is, elapsed: " << elapsed_secs);
std::this_thread::sleep_for(std::chrono::duration<double>(dt.toSec() - elapsed_secs));
}
rosgraph_msgs::Clock clock;
clock.clock = ros::Time(internal_time);
clock_publisher.publish(clock);
internal_time += dt;
}
spinner.stop();
return 0;
}

90
mir_robot/diff_drive_controller/test/skidsteerbot.xacro Executable file
View File

@@ -0,0 +1,90 @@
<?xml version="1.0"?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!--
Robot model taken from http://wiki.ros.org/pr2_mechanism/Tutorials/SImple%20URDF-Controller%20Example
-->
<xacro:include filename="$(find diff_drive_controller)/test/wheel.xacro"/>
<xacro:property name="deg_to_rad" value="0.01745329251994329577"/>
<!-- Constants for robot dimensions -->
<xacro:property name="width" value="0.5" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="wheel_radius" value="0.11" /> <!-- Link 1 -->
<xacro:property name="thickness" value="0.086" /> <!-- Link 2 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<xacro:property name="y_offset" value="0.35" /> <!-- Offset for the wheels on the same side -->
<!-- Links: inertial,visual,collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="${width} 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<!-- Wheels -->
<xacro:wheel name="wheel_0" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_1" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_2" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_3" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${-width/2+axel_offset} ${y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_4" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${-width/2+axel_offset} 0 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<xacro:wheel name="wheel_5" parent="base" radius="${wheel_radius}" thickness="${thickness}">
<origin xyz="${-width/2+axel_offset} ${-y_offset} 0" rpy="${-90 * deg_to_rad} 0 0"/>
</xacro:wheel>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
</robot>

8
mir_robot/diff_drive_controller/test/skidsteerbot_controllers.yaml Executable file
View File

@@ -0,0 +1,8 @@
skidsteerbot_controller:
type: "diff_drive_controller/DiffDriveController"
left_wheel: ['wheel_0_joint', 'wheel_1_joint', 'wheel_2_joint']
right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint']
publish_rate: 50.0 # defaults to 50
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

8
mir_robot/diff_drive_controller/test/skidsteerbot_no_wheels.yaml Executable file
View File

@@ -0,0 +1,8 @@
skidsteerbot_controller:
type: "diff_drive_controller/DiffDriveController"
left_wheel: []
right_wheel: ['wheel_3_joint', 'wheel_4_joint', 'wheel_5_joint']
publish_rate: 50.0 # defaults to 50
pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests

43
mir_robot/diff_drive_controller/test/sphere_wheel.xacro Executable file
View File

@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="sphere_wheel" params="name parent radius *origin">
<link name="${name}_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="continuous">
<parent link="${parent}_link"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${name}_joint_trans" type="SimpleTransmission">
<actuator name="${name}_joint_motor"/>
<joint name="${name}_joint"/>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<gazebo reference="${name}_link">
<material>Gazebo/Red</material>
</gazebo>
</xacro:macro>
</robot>

44
mir_robot/diff_drive_controller/test/square_wheel.xacro Executable file
View File

@@ -0,0 +1,44 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="square_wheel" params="name parent radius *origin">
<link name="${name}_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<!-- use boxes for wheels, scale radius to account for "circumference" difference -->
<box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${wheel_radius*1.11} ${wheel_radius*1.11} ${wheel_radius*1.11} "/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="continuous">
<parent link="${parent}_link"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${name}_joint_trans" type="SimpleTransmission">
<actuator name="${name}_joint_motor"/>
<joint name="${name}_joint"/>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<gazebo reference="${name}_link">
<material>Gazebo/Red</material>
</gazebo>
</xacro:macro>
</robot>

162
mir_robot/diff_drive_controller/test/test_common.h Executable file
View File

@@ -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);
}

26
mir_robot/diff_drive_controller/test/view_diffbot.launch Executable file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<launch>
<!-- start world -->
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
<!-- load robot -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/diffbot.xacro'" />
<!-- Start diffbot -->
<node name="diffbot"
pkg="diff_drive_controller"
type="diffbot"/>
<!-- Load controller config -->
<rosparam command="load" file="$(find diff_drive_controller)/test/diffbot_controllers.yaml" />
<!-- Spawn controller -->
<node name="controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="diffbot_controller" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model diffbot -z 0.5" respawn="false" output="screen" />
</launch>

26
mir_robot/diff_drive_controller/test/view_skidsteerbot.launch Executable file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<launch>
<!-- start world -->
<include file="$(find gazebo_worlds)/launch/empty_world.launch"/>
<!-- load robot -->
<param name="robot_description"
command="$(find xacro)/xacro '$(find diff_drive_controller)/test/skidsteerbot.xacro'" />
<!-- Start skidsteerbot -->
<node name="skidsteerbot"
pkg="diff_drive_controller"
type="skidsteerbot"/>
<!-- Load controller config -->
<rosparam command="load" file="$(find diff_drive_controller)/test/skidsteerbot_controllers.yaml" />
<!-- Spawn controller -->
<node name="controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="skidsteerbot_controller" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_single_link" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model skidsteerbot -z 0.5" respawn="false" output="screen" />
</launch>

43
mir_robot/diff_drive_controller/test/wheel.xacro Executable file
View File

@@ -0,0 +1,43 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="wheel" params="name parent radius thickness *origin">
<link name="${name}_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${thickness}"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder radius="${wheel_radius}" length="${thickness}"/>
</geometry>
</collision>
</link>
<joint name="${name}_joint" type="continuous">
<parent link="${parent}_link"/>
<child link="${name}_link"/>
<xacro:insert_block name="origin"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="${name}_joint_trans" type="SimpleTransmission">
<actuator name="${name}_joint_motor"/>
<joint name="${name}_joint"/>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<gazebo reference="${name}_link">
<material>Gazebo/Red</material>
</gazebo>
</xacro:macro>
</robot>