Skip to content

Commit 429453e

Browse files
bmagyarbmagyar
committed
Merge pull request #205 from tappan-at-git/indigo-devel
Parameterized diff_drive_controller's odom_frame_id
2 parents db6efa3 + 3c4f5d7 commit 429453e

File tree

7 files changed

+215
-2
lines changed

7 files changed

+215
-2
lines changed

‎diff_drive_controller/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -66,6 +66,14 @@ if (CATKIN_ENABLE_TESTING)
6666
test/diff_drive_odom_tf.test
6767
test/diff_drive_odom_tf_test.cpp)
6868
target_link_libraries(diff_drive_odom_tf_test ${catkin_LIBRARIES})
69+
add_rostest_gtest(diff_drive_default_odom_frame_test
70+
test/diff_drive_default_odom_frame.test
71+
test/diff_drive_default_odom_frame_test.cpp)
72+
target_link_libraries(diff_drive_default_odom_frame_test ${catkin_LIBRARIES})
73+
add_rostest_gtest(diff_drive_odom_frame_test
74+
test/diff_drive_odom_frame.test
75+
test/diff_drive_odom_frame_test.cpp)
76+
target_link_libraries(diff_drive_odom_frame_test ${catkin_LIBRARIES})
6977
add_rostest(test/diff_drive_open_loop.test)
7078
add_rostest(test/skid_steer_controller.test)
7179
add_rostest(test/skid_steer_no_wheels.test)

‎diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -144,6 +144,9 @@ namespace diff_drive_controller{
144144
/// Frame to use for the robot base:
145145
std::string base_frame_id_;
146146

147+
/// Frame to use for odometry and odom tf:
148+
std::string odom_frame_id_;
149+
147150
/// Whether to publish odometry to tf or not:
148151
bool enable_odom_tf_;
149152

‎diff_drive_controller/src/diff_drive_controller.cpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,7 @@ namespace diff_drive_controller{
117117
, cmd_vel_timeout_(0.5)
118118
, allow_multiple_cmd_vel_publishers_(true)
119119
, base_frame_id_("base_link")
120+
, odom_frame_id_("odom")
120121
, enable_odom_tf_(true)
121122
, wheel_joints_size_(0)
122123
{
@@ -189,6 +190,9 @@ namespace diff_drive_controller{
189190
controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
190191
ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
191192

193+
controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
194+
ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
195+
192196
controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
193197
ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
194198

@@ -544,7 +548,7 @@ namespace diff_drive_controller{
544548

545549
// Setup odometry realtime publisher + odom message constant fields
546550
odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
547-
odom_pub_->msg_.header.frame_id = "odom";
551+
odom_pub_->msg_.header.frame_id = odom_frame_id_;
548552
odom_pub_->msg_.child_frame_id = base_frame_id_;
549553
odom_pub_->msg_.pose.pose.position.z = 0;
550554
odom_pub_->msg_.pose.covariance = boost::assign::list_of
@@ -569,7 +573,7 @@ namespace diff_drive_controller{
569573
tf_odom_pub_->msg_.transforms.resize(1);
570574
tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
571575
tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
572-
tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom";
576+
tf_odom_pub_->msg_.transforms[0].header.frame_id = odom_frame_id_;
573577
}
574578

575579
} // namespace diff_drive_controller

‎diff_drive_controller/test/diff_drive_default_odom_frame.test‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<launch>
2+
<!-- Load common test stuff -->
3+
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
4+
5+
<!-- Do not specific an odom_frame_id -->
6+
<!-- <rosparam>
7+
diffbot_controller:
8+
odom_frame_id: odom
9+
</rosparam> -->
10+
11+
<!-- Controller test -->
12+
<test test-name="diff_drive_default_odom_frame_test"
13+
pkg="diff_drive_controller"
14+
type="diff_drive_default_odom_frame_test"
15+
time-limit="80.0">
16+
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
17+
<remap from="odom" to="diffbot_controller/odom" />
18+
</test>
19+
</launch>

‎diff_drive_controller/test/diff_drive_default_odom_frame_test.cpp‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
///////////////////////////////////////////////////////////////////////////////
2+
// Copyright (C) 2015, Locus Robotics Corp.
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
// * Redistributions of source code must retain the above copyright notice,
7+
// this list of conditions and the following disclaimer.
8+
// * Redistributions in binary form must reproduce the above copyright
9+
// notice, this list of conditions and the following disclaimer in the
10+
// documentation and/or other materials provided with the distribution.
11+
// * Neither the name of PAL Robotics, Inc. nor the names of its
12+
// contributors may be used to endorse or promote products derived from
13+
// this software without specific prior written permission.
14+
//
15+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25+
// POSSIBILITY OF SUCH DAMAGE.
26+
//////////////////////////////////////////////////////////////////////////////
27+
28+
/// \author Eric Tappan
29+
30+
#include "test_common.h"
31+
#include <tf/transform_listener.h>
32+
33+
// TEST CASES
34+
TEST_F(DiffDriveControllerTest, testOdomFrame)
35+
{
36+
// wait for ROS
37+
while(!isControllerAlive())
38+
{
39+
ros::Duration(0.1).sleep();
40+
}
41+
// set up tf listener
42+
tf::TransformListener listener;
43+
ros::Duration(2.0).sleep();
44+
// check the original odom frame doesn't exist
45+
EXPECT_TRUE(listener.frameExists("odom"));
46+
}
47+
48+
TEST_F(DiffDriveControllerTest, testOdomTopic)
49+
{
50+
// wait for ROS
51+
while(!isControllerAlive())
52+
{
53+
ros::Duration(0.1).sleep();
54+
}
55+
56+
// get an odom message
57+
nav_msgs::Odometry odom_msg = getLastOdom();
58+
// check its frame_id
59+
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "odom");
60+
}
61+
62+
int main(int argc, char** argv)
63+
{
64+
testing::InitGoogleTest(&argc, argv);
65+
ros::init(argc, argv, "diff_drive_default_odom_frame_test");
66+
67+
ros::AsyncSpinner spinner(1);
68+
spinner.start();
69+
int ret = RUN_ALL_TESTS();
70+
spinner.stop();
71+
ros::shutdown();
72+
return ret;
73+
}

‎diff_drive_controller/test/diff_drive_odom_frame.test‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
<launch>
2+
<!-- Load common test stuff -->
3+
<include file="$(find diff_drive_controller)/test/diff_drive_common.launch" />
4+
5+
<!-- Set odom_frame_id to something new -->
6+
<rosparam>
7+
diffbot_controller:
8+
odom_frame_id: new_odom
9+
</rosparam>
10+
11+
<!-- Controller test -->
12+
<test test-name="diff_drive_odom_frame_test"
13+
pkg="diff_drive_controller"
14+
type="diff_drive_odom_frame_test"
15+
time-limit="80.0">
16+
<remap from="cmd_vel" to="diffbot_controller/cmd_vel" />
17+
<remap from="odom" to="diffbot_controller/odom" />
18+
</test>
19+
</launch>

‎diff_drive_controller/test/diff_drive_odom_frame_test.cpp‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
///////////////////////////////////////////////////////////////////////////////
2+
// Copyright (C) 2015, Locus Robotics Corp.
3+
//
4+
// Redistribution and use in source and binary forms, with or without
5+
// modification, are permitted provided that the following conditions are met:
6+
// * Redistributions of source code must retain the above copyright notice,
7+
// this list of conditions and the following disclaimer.
8+
// * Redistributions in binary form must reproduce the above copyright
9+
// notice, this list of conditions and the following disclaimer in the
10+
// documentation and/or other materials provided with the distribution.
11+
// * Neither the name of PAL Robotics, Inc. nor the names of its
12+
// contributors may be used to endorse or promote products derived from
13+
// this software without specific prior written permission.
14+
//
15+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16+
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17+
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18+
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19+
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20+
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21+
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22+
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23+
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24+
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25+
// POSSIBILITY OF SUCH DAMAGE.
26+
//////////////////////////////////////////////////////////////////////////////
27+
28+
/// \author Eric Tappan
29+
30+
#include "test_common.h"
31+
#include <tf/transform_listener.h>
32+
33+
// TEST CASES
34+
TEST_F(DiffDriveControllerTest, testNoOdomFrame)
35+
{
36+
// wait for ROS
37+
while(!isControllerAlive())
38+
{
39+
ros::Duration(0.1).sleep();
40+
}
41+
// set up tf listener
42+
tf::TransformListener listener;
43+
ros::Duration(2.0).sleep();
44+
// check the original odom frame doesn't exist
45+
EXPECT_FALSE(listener.frameExists("odom"));
46+
}
47+
48+
TEST_F(DiffDriveControllerTest, testNewOdomFrame)
49+
{
50+
// wait for ROS
51+
while(!isControllerAlive())
52+
{
53+
ros::Duration(0.1).sleep();
54+
}
55+
// set up tf listener
56+
tf::TransformListener listener;
57+
ros::Duration(2.0).sleep();
58+
// check the new_odom frame does exist
59+
EXPECT_TRUE(listener.frameExists("new_odom"));
60+
}
61+
62+
TEST_F(DiffDriveControllerTest, testOdomTopic)
63+
{
64+
// wait for ROS
65+
while(!isControllerAlive())
66+
{
67+
ros::Duration(0.1).sleep();
68+
}
69+
70+
// get an odom message
71+
nav_msgs::Odometry odom_msg = getLastOdom();
72+
// check its frame_id
73+
ASSERT_STREQ(odom_msg.header.frame_id.c_str(), "new_odom");
74+
}
75+
76+
int main(int argc, char** argv)
77+
{
78+
testing::InitGoogleTest(&argc, argv);
79+
ros::init(argc, argv, "diff_drive_odom_frame_test");
80+
81+
ros::AsyncSpinner spinner(1);
82+
spinner.start();
83+
int ret = RUN_ALL_TESTS();
84+
spinner.stop();
85+
ros::shutdown();
86+
return ret;
87+
}

0 commit comments

Comments
 (0)