Skip to content
This repository was archived by the owner on Sep 2, 2022. It is now read-only.

Commit fdc0ad1

Browse files
authored
Setup framework, add Dubins curve approximation interactive test (#1)
* initial setup commit Signed-off-by: ddengster <ed.fan@osrfoundation.org> * basic circular approximation impl with 2 test cases Signed-off-by: ddengster <ed.fan@osrfoundation.org> * add more examples, fix issues with clockwise/anticlockwise motions Signed-off-by: ddengster <ed.fan@osrfoundation.org> * seperate curve motion and rename variables Signed-off-by: ddengster <ed.fan@osrfoundation.org>
1 parent c125aee commit fdc0ad1

File tree

10 files changed

+1357
-1
lines changed

10 files changed

+1357
-1
lines changed

CMakeLists.txt

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
cmake_minimum_required(VERSION 3.5.0)
2+
3+
project(rmf_geometry_testbed)
4+
5+
if (NOT CMAKE_CXX_STANDARD)
6+
set(CMAKE_CXX_STANDARD 14)
7+
endif()
8+
9+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
10+
add_compile_options(-Wall -Wextra -Wpedantic)
11+
endif()
12+
13+
find_package(ament_cmake REQUIRED)
14+
# find_package(rmf_fleet_adapter REQUIRED)
15+
find_package(rmf_traffic REQUIRED)
16+
find_package(SFML COMPONENTS graphics window system REQUIRED)
17+
find_package(ImGui-SFML)
18+
19+
20+
include(GNUInstallDirs)
21+
22+
add_library(
23+
rmf_geometry_utils SHARED
24+
src/rmf_geometry_testbed/IMDraw.cpp
25+
)
26+
27+
target_link_libraries(
28+
rmf_geometry_utils
29+
PUBLIC
30+
rmf_traffic::rmf_traffic
31+
sfml-graphics
32+
)
33+
34+
target_include_directories(
35+
rmf_geometry_utils
36+
PUBLIC
37+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
38+
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
39+
)
40+
41+
######## FCL test ##########
42+
43+
find_package(fcl 0.6 QUIET)
44+
if(fcl_FOUND)
45+
set(FCL_LIBRARIES fcl)
46+
message(STATUS "Using FCL version: ${FCL_VERSION}")
47+
48+
add_executable(test_reedshepp_cubic
49+
test/test_reedshepp_cubic.cpp
50+
test/ReedsShepp.cpp
51+
)
52+
53+
target_link_libraries(
54+
test_reedshepp_cubic
55+
PUBLIC
56+
rmf_geometry_utils
57+
ImGui-SFML::ImGui-SFML
58+
fcl
59+
)
60+
endif()
61+
62+
## copy font files
63+
install(
64+
DIRECTORY fonts/
65+
DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/fonts/
66+
)
67+
68+

README.md

+13-1
Original file line numberDiff line numberDiff line change
@@ -1 +1,13 @@
1-
# rmf_geometry_testbed
1+
# rmf_geometry_testbed: Interactive tests for testing collision detection, collision primitives approximation
2+
3+
1. Create a new colcon workspace
4+
2. Clone this repo
5+
3. Clone rmf repositories https://github.com/open-rmf/rmf_utils.git, https://github.com/open-rmf/rmf_traffic.git
6+
4. Clone fcl 6.0 https://github.com/flexible-collision-library/fcl.git
7+
4. Clone https://github.com/SFML/SFML
8+
5. Clone https://github.com/eliasdaler/imgui-sfml.git
9+
6. Clone the version of https://github.com/ocornut/imgui.git mentioned in the previous step (at the time of this writing, v1.80)
10+
7. go into your imgui directory and do `touch COLCON_IGNORE`
11+
8. `colcon build --cmake-args -DBUILD_SHARED_LIBS=ON -DIMGUI_DIR=<your colcon workspace>/src/imgui`
12+
13+
Then you can start running interactive tests in the `./build/rmf_geometry_testbed/` directory

fonts/LICENSE.txt

+202
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,202 @@
1+
2+
Apache License
3+
Version 2.0, January 2004
4+
http://www.apache.org/licenses/
5+
6+
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
7+
8+
1. Definitions.
9+
10+
"License" shall mean the terms and conditions for use, reproduction,
11+
and distribution as defined by Sections 1 through 9 of this document.
12+
13+
"Licensor" shall mean the copyright owner or entity authorized by
14+
the copyright owner that is granting the License.
15+
16+
"Legal Entity" shall mean the union of the acting entity and all
17+
other entities that control, are controlled by, or are under common
18+
control with that entity. For the purposes of this definition,
19+
"control" means (i) the power, direct or indirect, to cause the
20+
direction or management of such entity, whether by contract or
21+
otherwise, or (ii) ownership of fifty percent (50%) or more of the
22+
outstanding shares, or (iii) beneficial ownership of such entity.
23+
24+
"You" (or "Your") shall mean an individual or Legal Entity
25+
exercising permissions granted by this License.
26+
27+
"Source" form shall mean the preferred form for making modifications,
28+
including but not limited to software source code, documentation
29+
source, and configuration files.
30+
31+
"Object" form shall mean any form resulting from mechanical
32+
transformation or translation of a Source form, including but
33+
not limited to compiled object code, generated documentation,
34+
and conversions to other media types.
35+
36+
"Work" shall mean the work of authorship, whether in Source or
37+
Object form, made available under the License, as indicated by a
38+
copyright notice that is included in or attached to the work
39+
(an example is provided in the Appendix below).
40+
41+
"Derivative Works" shall mean any work, whether in Source or Object
42+
form, that is based on (or derived from) the Work and for which the
43+
editorial revisions, annotations, elaborations, or other modifications
44+
represent, as a whole, an original work of authorship. For the purposes
45+
of this License, Derivative Works shall not include works that remain
46+
separable from, or merely link (or bind by name) to the interfaces of,
47+
the Work and Derivative Works thereof.
48+
49+
"Contribution" shall mean any work of authorship, including
50+
the original version of the Work and any modifications or additions
51+
to that Work or Derivative Works thereof, that is intentionally
52+
submitted to Licensor for inclusion in the Work by the copyright owner
53+
or by an individual or Legal Entity authorized to submit on behalf of
54+
the copyright owner. For the purposes of this definition, "submitted"
55+
means any form of electronic, verbal, or written communication sent
56+
to the Licensor or its representatives, including but not limited to
57+
communication on electronic mailing lists, source code control systems,
58+
and issue tracking systems that are managed by, or on behalf of, the
59+
Licensor for the purpose of discussing and improving the Work, but
60+
excluding communication that is conspicuously marked or otherwise
61+
designated in writing by the copyright owner as "Not a Contribution."
62+
63+
"Contributor" shall mean Licensor and any individual or Legal Entity
64+
on behalf of whom a Contribution has been received by Licensor and
65+
subsequently incorporated within the Work.
66+
67+
2. Grant of Copyright License. Subject to the terms and conditions of
68+
this License, each Contributor hereby grants to You a perpetual,
69+
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
70+
copyright license to reproduce, prepare Derivative Works of,
71+
publicly display, publicly perform, sublicense, and distribute the
72+
Work and such Derivative Works in Source or Object form.
73+
74+
3. Grant of Patent License. Subject to the terms and conditions of
75+
this License, each Contributor hereby grants to You a perpetual,
76+
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
77+
(except as stated in this section) patent license to make, have made,
78+
use, offer to sell, sell, import, and otherwise transfer the Work,
79+
where such license applies only to those patent claims licensable
80+
by such Contributor that are necessarily infringed by their
81+
Contribution(s) alone or by combination of their Contribution(s)
82+
with the Work to which such Contribution(s) was submitted. If You
83+
institute patent litigation against any entity (including a
84+
cross-claim or counterclaim in a lawsuit) alleging that the Work
85+
or a Contribution incorporated within the Work constitutes direct
86+
or contributory patent infringement, then any patent licenses
87+
granted to You under this License for that Work shall terminate
88+
as of the date such litigation is filed.
89+
90+
4. Redistribution. You may reproduce and distribute copies of the
91+
Work or Derivative Works thereof in any medium, with or without
92+
modifications, and in Source or Object form, provided that You
93+
meet the following conditions:
94+
95+
(a) You must give any other recipients of the Work or
96+
Derivative Works a copy of this License; and
97+
98+
(b) You must cause any modified files to carry prominent notices
99+
stating that You changed the files; and
100+
101+
(c) You must retain, in the Source form of any Derivative Works
102+
that You distribute, all copyright, patent, trademark, and
103+
attribution notices from the Source form of the Work,
104+
excluding those notices that do not pertain to any part of
105+
the Derivative Works; and
106+
107+
(d) If the Work includes a "NOTICE" text file as part of its
108+
distribution, then any Derivative Works that You distribute must
109+
include a readable copy of the attribution notices contained
110+
within such NOTICE file, excluding those notices that do not
111+
pertain to any part of the Derivative Works, in at least one
112+
of the following places: within a NOTICE text file distributed
113+
as part of the Derivative Works; within the Source form or
114+
documentation, if provided along with the Derivative Works; or,
115+
within a display generated by the Derivative Works, if and
116+
wherever such third-party notices normally appear. The contents
117+
of the NOTICE file are for informational purposes only and
118+
do not modify the License. You may add Your own attribution
119+
notices within Derivative Works that You distribute, alongside
120+
or as an addendum to the NOTICE text from the Work, provided
121+
that such additional attribution notices cannot be construed
122+
as modifying the License.
123+
124+
You may add Your own copyright statement to Your modifications and
125+
may provide additional or different license terms and conditions
126+
for use, reproduction, or distribution of Your modifications, or
127+
for any such Derivative Works as a whole, provided Your use,
128+
reproduction, and distribution of the Work otherwise complies with
129+
the conditions stated in this License.
130+
131+
5. Submission of Contributions. Unless You explicitly state otherwise,
132+
any Contribution intentionally submitted for inclusion in the Work
133+
by You to the Licensor shall be under the terms and conditions of
134+
this License, without any additional terms or conditions.
135+
Notwithstanding the above, nothing herein shall supersede or modify
136+
the terms of any separate license agreement you may have executed
137+
with Licensor regarding such Contributions.
138+
139+
6. Trademarks. This License does not grant permission to use the trade
140+
names, trademarks, service marks, or product names of the Licensor,
141+
except as required for reasonable and customary use in describing the
142+
origin of the Work and reproducing the content of the NOTICE file.
143+
144+
7. Disclaimer of Warranty. Unless required by applicable law or
145+
agreed to in writing, Licensor provides the Work (and each
146+
Contributor provides its Contributions) on an "AS IS" BASIS,
147+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
148+
implied, including, without limitation, any warranties or conditions
149+
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
150+
PARTICULAR PURPOSE. You are solely responsible for determining the
151+
appropriateness of using or redistributing the Work and assume any
152+
risks associated with Your exercise of permissions under this License.
153+
154+
8. Limitation of Liability. In no event and under no legal theory,
155+
whether in tort (including negligence), contract, or otherwise,
156+
unless required by applicable law (such as deliberate and grossly
157+
negligent acts) or agreed to in writing, shall any Contributor be
158+
liable to You for damages, including any direct, indirect, special,
159+
incidental, or consequential damages of any character arising as a
160+
result of this License or out of the use or inability to use the
161+
Work (including but not limited to damages for loss of goodwill,
162+
work stoppage, computer failure or malfunction, or any and all
163+
other commercial damages or losses), even if such Contributor
164+
has been advised of the possibility of such damages.
165+
166+
9. Accepting Warranty or Additional Liability. While redistributing
167+
the Work or Derivative Works thereof, You may choose to offer,
168+
and charge a fee for, acceptance of support, warranty, indemnity,
169+
or other liability obligations and/or rights consistent with this
170+
License. However, in accepting such obligations, You may act only
171+
on Your own behalf and on Your sole responsibility, not on behalf
172+
of any other Contributor, and only if You agree to indemnify,
173+
defend, and hold each Contributor harmless for any liability
174+
incurred by, or claims asserted against, such Contributor by reason
175+
of your accepting any such warranty or additional liability.
176+
177+
END OF TERMS AND CONDITIONS
178+
179+
APPENDIX: How to apply the Apache License to your work.
180+
181+
To apply the Apache License to your work, attach the following
182+
boilerplate notice, with the fields enclosed by brackets "[]"
183+
replaced with your own identifying information. (Don't include
184+
the brackets!) The text should be enclosed in the appropriate
185+
comment syntax for the file format. We also recommend that a
186+
file or class name and description of purpose be included on the
187+
same "printed page" as the copyright notice for easier
188+
identification within third-party archives.
189+
190+
Copyright [yyyy] [name of copyright owner]
191+
192+
Licensed under the Apache License, Version 2.0 (the "License");
193+
you may not use this file except in compliance with the License.
194+
You may obtain a copy of the License at
195+
196+
http://www.apache.org/licenses/LICENSE-2.0
197+
198+
Unless required by applicable law or agreed to in writing, software
199+
distributed under the License is distributed on an "AS IS" BASIS,
200+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
201+
See the License for the specific language governing permissions and
202+
limitations under the License.

fonts/OpenSans-Bold.ttf

102 KB
Binary file not shown.
+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
2+
/*
3+
* Copyright (C) 2021 Open Source Robotics Foundation
4+
*
5+
* Licensed under the Apache License, Version 2.0 (the "License");
6+
* you may not use this file except in compliance with the License.
7+
* You may obtain a copy of the License at
8+
*
9+
* http://www.apache.org/licenses/LICENSE-2.0
10+
*
11+
* Unless required by applicable law or agreed to in writing, software
12+
* distributed under the License is distributed on an "AS IS" BASIS,
13+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14+
* See the License for the specific language governing permissions and
15+
* limitations under the License.
16+
*
17+
*/
18+
19+
#ifndef RMF_GEOMETRY_TESTBED__DRAW__IMDRAW_HPP
20+
#define RMF_GEOMETRY_TESTBED__DRAW__IMDRAW_HPP
21+
22+
#include <SFML/Graphics.hpp>
23+
#include <Eigen/Dense>
24+
25+
#include <rmf_traffic/Trajectory.hpp>
26+
27+
namespace rmf_geometry_utils {
28+
29+
/// Series of immediate-mode drawing functions. Note that this is done for iteration times and quick access, and may not be profile
30+
class IMDraw
31+
{
32+
public:
33+
static void draw_trajectory(const rmf_traffic::Trajectory& trajectory,
34+
const sf::Color& color = sf::Color(255, 255, 255, 255));
35+
36+
static void draw_sampled_trajectory(const rmf_traffic::Trajectory& trajectory,
37+
double seconds,
38+
const sf::Color& color = sf::Color(255, 255, 255, 255));
39+
40+
static void draw_circle(
41+
const sf::Vector2f& center, double radius,
42+
const sf::Color& color = sf::Color(255, 255, 255, 255),
43+
uint slices = 16);
44+
45+
static void draw_axis(float size = 1.0f);
46+
47+
static void draw_line(const sf::Vector2f& start, const sf::Vector2f& end, const sf::Color& color = sf::Color(255, 255, 255, 255));
48+
49+
static void draw_arrow(const sf::Vector2f& start, const sf::Vector2f& end, const sf::Color& color = sf::Color(255, 255, 255, 255));
50+
51+
static void draw_aabb(const sf::Vector2f& box_min, const sf::Vector2f& box_max, const sf::Color& color = sf::Color(255, 255, 255, 255));
52+
53+
// render all objects and flush the internal vertex array buffer
54+
static void flush_and_render(sf::RenderWindow& app_window, const sf::Transform& tx_flipped_2d);
55+
56+
57+
/***************** Eigen versions *****************/
58+
static sf::Vector2f toSfVector2(Eigen::Vector2d v)
59+
{
60+
return sf::Vector2f(v.x(), v.y());
61+
}
62+
63+
static void draw_circle(
64+
Eigen::Vector2d center, double radius,
65+
const sf::Color& color = sf::Color(255, 255, 255, 255),
66+
uint slices = 16)
67+
{ IMDraw::draw_circle(toSfVector2(center), radius, color, slices); }
68+
69+
static inline void draw_line(Eigen::Vector2d start, Eigen::Vector2d end, const sf::Color& color = sf::Color(255, 255, 255, 255))
70+
{ IMDraw::draw_line(toSfVector2(start), toSfVector2(end), color); }
71+
72+
static inline void draw_arrow(Eigen::Vector2d start, Eigen::Vector2d end, const sf::Color& color = sf::Color(255, 255, 255, 255))
73+
{ IMDraw::draw_arrow(toSfVector2(start), toSfVector2(end), color); }
74+
75+
static inline void draw_aabb(Eigen::Vector2d box_min, Eigen::Vector2d box_max, const sf::Color& color = sf::Color(255, 255, 255, 255))
76+
{ IMDraw::draw_aabb(toSfVector2(box_min), toSfVector2(box_max), color); }
77+
};
78+
79+
80+
} // namespace rmf_geometry_utils
81+
82+
#endif // RMF_PLANNER_VIZ__DRAW__IMDRAW_HPP

package.xml

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>rmf_geometry_testbed</name>
5+
<version>0.0.1</version>
6+
<description>Geometry visualization testbed</description>
7+
<maintainer email="ed.fan@openrobotics.org">Grey</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rmf_traffic</depend>
13+
<depend>SFML</depend>
14+
<depend>imgui_sfml</depend>
15+
<depend>fcl</depend>
16+
17+
<export>
18+
<build_type>ament_cmake</build_type>
19+
</export>
20+
</package>
21+
22+

0 commit comments

Comments
 (0)