Skip to content

Commit cf3e655

Browse files
rayferricrayferric
committed
merge main
2 parents 9b2c9c7 + d6e7595 commit cf3e655

88 files changed

Lines changed: 3544 additions & 221 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎kalman_arc/CMakeLists.txt‎

Lines changed: 168 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,18 +18,186 @@ endif()
1818
find_package(rclcpp REQUIRED)
1919
find_package(std_msgs REQUIRED)
2020
find_package(nav_msgs REQUIRED)
21+
find_package(Protobuf REQUIRED)
22+
find_package(kalman_interfaces REQUIRED)
23+
find_package(pluginlib REQUIRED)
24+
find_package(Qt5 REQUIRED COMPONENTS Widgets)
25+
find_package(qt_gui_cpp REQUIRED)
26+
find_package(rqt_gui_cpp REQUIRED)
27+
28+
# darkest rock finder deps
29+
find_package(rclcpp_components REQUIRED)
30+
find_package(pcl_ros REQUIRED)
31+
find_package(pcl_conversions REQUIRED)
32+
find_package(vision_msgs REQUIRED)
33+
find_package(tf2_ros REQUIRED)
34+
find_package(tf2_geometry_msgs REQUIRED)
35+
find_package(std_srvs REQUIRED)
36+
37+
add_library(rscp_proto_messages
38+
proto/rscp.proto
39+
)
40+
41+
set_target_properties(rscp_proto_messages PROPERTIES POSITION_INDEPENDENT_CODE ON)
42+
43+
protobuf_generate(
44+
TARGET rscp_proto_messages
45+
LANGUAGE cpp
46+
PROTOS proto/rscp.proto
47+
)
48+
49+
target_link_libraries(rscp_proto_messages
50+
protobuf::libprotobuf
51+
)
52+
53+
target_include_directories(rscp_proto_messages
54+
PUBLIC
55+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
56+
$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>
57+
$<INSTALL_INTERFACE:include>
58+
)
59+
60+
add_library(kalman_arc_cobs
61+
src/cobs.cpp
62+
)
63+
64+
set_target_properties(kalman_arc_cobs PROPERTIES POSITION_INDEPENDENT_CODE ON)
65+
66+
target_include_directories(kalman_arc_cobs
67+
PUBLIC
68+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
69+
$<INSTALL_INTERFACE:include>
70+
)
71+
72+
add_executable(rscp_node src/proto_node.cpp)
73+
74+
target_link_libraries(rscp_node
75+
kalman_arc_cobs
76+
rscp_proto_messages
77+
)
78+
79+
ament_target_dependencies(rscp_node
80+
rclcpp
81+
std_msgs
82+
kalman_interfaces
83+
)
84+
85+
add_library(rqt_rscp SHARED
86+
include/kalman_arc/rqt_rscp.hpp
87+
src/rqt_rscp.cpp
88+
)
89+
90+
set_target_properties(rqt_rscp PROPERTIES AUTOMOC ON)
91+
92+
target_include_directories(rqt_rscp
93+
PRIVATE
94+
${CMAKE_CURRENT_SOURCE_DIR}/include
95+
${CMAKE_CURRENT_BINARY_DIR}
96+
)
97+
98+
target_link_libraries(rqt_rscp
99+
Qt5::Widgets
100+
)
101+
102+
ament_target_dependencies(rqt_rscp
103+
pluginlib
104+
qt_gui_cpp
105+
rclcpp
106+
rqt_gui_cpp
107+
kalman_interfaces
108+
)
109+
find_package(sensor_msgs REQUIRED)
110+
find_package(geometry_msgs REQUIRED)
111+
find_package(tf2_ros REQUIRED)
112+
find_package(tf2_sensor_msgs REQUIRED)
113+
find_package(grid_map_ros REQUIRED)
114+
find_package(grid_map_core REQUIRED)
115+
find_package(grid_map_msgs REQUIRED)
116+
find_package(rclcpp_components REQUIRED)
117+
find_package(std_srvs REQUIRED)
118+
119+
add_library(peak_finder_node SHARED src/peak_finder_node.cpp)
120+
ament_target_dependencies(peak_finder_node
121+
rclcpp
122+
rclcpp_components
123+
sensor_msgs
124+
tf2_ros
125+
tf2_sensor_msgs
126+
grid_map_ros
127+
grid_map_core
128+
grid_map_msgs
129+
geometry_msgs
130+
std_srvs
131+
)
132+
rclcpp_components_register_node(
133+
peak_finder_node
134+
PLUGIN "kalman_arc::PeakFinder"
135+
EXECUTABLE peak_finder
136+
)
137+
install(TARGETS peak_finder_node peak_finder
138+
EXPORT kalman_arc-targets
139+
RUNTIME DESTINATION bin
140+
LIBRARY DESTINATION lib
141+
ARCHIVE DESTINATION lib
142+
)
143+
144+
# >>> darkest rock finder
145+
146+
add_library(darkest_rock_finder_node SHARED src/darkest_rock_finder_node.cpp)
147+
ament_target_dependencies(darkest_rock_finder_node
148+
rclcpp
149+
rclcpp_components
150+
pcl_ros
151+
pcl_conversions
152+
vision_msgs
153+
tf2_ros
154+
tf2_geometry_msgs
155+
std_srvs
156+
# This dependency is completely unnnecessary, but compilation fails without it. I don't know why. I don't think I want to know:
157+
grid_map_core # 67
158+
)
159+
rclcpp_components_register_node(
160+
darkest_rock_finder_node
161+
PLUGIN "rock_finder::DarkestRockFinder"
162+
EXECUTABLE darkest_rock_finder
163+
)
164+
165+
# <<< darkest rock finder
21166

22167
install(PROGRAMS
23168
scripts/travel_distance_meter
24169
DESTINATION lib/${PROJECT_NAME}
25170
)
26171

172+
install(TARGETS
173+
kalman_arc_cobs
174+
rscp_proto_messages
175+
rscp_node
176+
rqt_rscp
177+
darkest_rock_finder_node
178+
ARCHIVE DESTINATION lib
179+
LIBRARY DESTINATION lib
180+
RUNTIME DESTINATION lib/${PROJECT_NAME}
181+
)
182+
27183
install(DIRECTORY launch
28184
DESTINATION share/${PROJECT_NAME}
29185
)
30186

187+
pluginlib_export_plugin_description_file(rqt_gui plugin.xml)
188+
31189
if(BUILD_TESTING)
190+
find_package(ament_cmake_gtest REQUIRED)
191+
192+
ament_add_gtest(cobs_test test/cobs_test.cpp)
193+
target_link_libraries(cobs_test
194+
kalman_arc_cobs
195+
)
196+
32197
find_package(ament_lint_auto REQUIRED)
198+
set(ament_cmake_copyright_FOUND TRUE)
199+
set(ament_cmake_cpplint_FOUND TRUE)
200+
set(ament_cmake_uncrustify_FOUND TRUE)
33201
ament_lint_auto_find_test_dependencies()
34202
endif()
35203

‎kalman_arc/README.md‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
# kalman_arc
2+
3+
Package for ARC competition functionality.
4+
5+
## RSCP rqt publisher
6+
7+
The package includes a development-only rqt plugin that builds an RSCP
8+
`RequestEnvelope`, serializes it, applies COBS framing, appends the `0x00`
9+
delimiter, and publishes the resulting `std_msgs/msg/UInt8MultiArray` to
10+
`rscp/serial/rx`.
11+
12+
After building and sourcing the workspace, start it with:
13+
14+
```bash
15+
rqt --standalone kalman_arc/RscpPublisher
16+
```
17+
18+
It is also available in the regular rqt plugin menu under
19+
`Plugins > Kalman > RSCP Publisher`.

‎kalman_arc/include/kalman_arc/cobs.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
#pragma once
2+
#include <cstddef>
3+
#include <cstdint>
4+
#include <vector>
5+
6+
std::vector<uint8_t> cobs_encode(const uint8_t *input, size_t length);
7+
std::vector<uint8_t> cobs_decode(const uint8_t *input, size_t length);

‎kalman_arc/include/kalman_arc/rqt_rscp.hpp‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
#pragma once
2+
3+
#include <QWidget>
4+
#include <kalman_interfaces/msg/arc_rscp_request.hpp>
5+
#include <kalman_interfaces/msg/detail/arc_rscp_request__struct.hpp>
6+
#include <rclcpp/publisher.hpp>
7+
#include <rqt_gui_cpp/plugin.h>
8+
#include <std_msgs/msg/u_int8_multi_array.hpp>
9+
10+
class QCheckBox;
11+
class QComboBox;
12+
class QDoubleSpinBox;
13+
class QLabel;
14+
class QSpinBox;
15+
16+
namespace kalman_arc {
17+
18+
class RqtRscp final : public rqt_gui_cpp::Plugin {
19+
Q_OBJECT
20+
21+
public:
22+
RqtRscp();
23+
24+
void initPlugin(qt_gui_cpp::PluginContext &context) override;
25+
void shutdownPlugin() override;
26+
void saveSettings(
27+
qt_gui_cpp::Settings &plugin_settings,
28+
qt_gui_cpp::Settings &instance_settings
29+
) const override;
30+
void restoreSettings(
31+
const qt_gui_cpp::Settings &plugin_settings,
32+
const qt_gui_cpp::Settings &instance_settings
33+
) override;
34+
35+
private slots:
36+
void update_visible_fields();
37+
void update_stage_name(int stage);
38+
void publish_request();
39+
40+
private:
41+
using ArcRscpRequest = kalman_interfaces::msg::ArcRscpRequest;
42+
43+
QWidget *widget_ = nullptr;
44+
QComboBox *request_type_ = nullptr;
45+
QCheckBox *arm_ = nullptr;
46+
QSpinBox *stage_ = nullptr;
47+
QLabel *stage_name_ = nullptr;
48+
QDoubleSpinBox *latitude_ = nullptr;
49+
QDoubleSpinBox *longitude_ = nullptr;
50+
QDoubleSpinBox *altitude_ = nullptr;
51+
QDoubleSpinBox *radius_ = nullptr;
52+
QLabel *status_ = nullptr;
53+
54+
QWidget *arm_row_ = nullptr;
55+
QWidget *stage_row_ = nullptr;
56+
QWidget *latitude_row_ = nullptr;
57+
QWidget *longitude_row_ = nullptr;
58+
QWidget *altitude_row_ = nullptr;
59+
QWidget *radius_row_ = nullptr;
60+
61+
rclcpp::Publisher<ArcRscpRequest>::SharedPtr publisher_;
62+
};
63+
64+
} // namespace kalman_arc

‎kalman_arc/kalman_arc/travel_distance_meter_node.py‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,16 @@
77

88
from launch.substitutions import LaunchConfiguration
99

10+
1011
class TravelDistanceMeterNode(Node):
1112
def __init__(self):
1213
super().__init__("travel_distance_meter_node")
1314

1415
self.declare_parameter("deadzone", 0.01)
15-
self.deadzone = self.get_parameter("deadzone").get_parameter_value().double_value
16-
16+
self.deadzone = (
17+
self.get_parameter("deadzone").get_parameter_value().double_value
18+
)
19+
1720
publish_period = 1
1821
self.total_distance = 0.0
1922
self.last_x = None

‎kalman_arc/launch/arc.launch.py‎

Lines changed: 38 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -5,28 +5,60 @@
55
OpaqueFunction,
66
)
77
from launch.substitutions import LaunchConfiguration
8+
9+
810
def launch_setup(context):
9-
deadzone_val = LaunchConfiguration("deadzone").perform(context)
11+
def get_bool(name):
12+
return LaunchConfiguration(name).perform(context).lower() == "true"
13+
14+
deadzone_val = LaunchConfiguration("travel_distance_meter_deadzone").perform(
15+
context
16+
)
1017
description = [
1118
Node(
1219
package="kalman_arc",
1320
executable="travel_distance_meter",
1421
name="travel_distance_meter_node",
1522
parameters=[{"deadzone": float(deadzone_val)}],
16-
output="screen"
17-
)
23+
output="screen",
24+
),
25+
Node(
26+
package="kalman_arc",
27+
executable="darkest_rock_finder",
28+
),
29+
Node(
30+
package="kalman_arc",
31+
executable="peak_finder",
32+
name="peak_finder_node",
33+
parameters=[{"map_resolution": 0.3}],
34+
output="screen",
35+
),
1836
]
1937

38+
if get_bool("enable_rscp_hw_driver"):
39+
description += [
40+
Node(
41+
package="kalman_arc",
42+
executable="rscp_node",
43+
),
44+
]
45+
2046
return description
2147

48+
2249
def generate_launch_description():
2350
return LaunchDescription(
2451
[
25-
DeclareLaunchArgument(
26-
"deadzone",
52+
DeclareLaunchArgument(
53+
"travel_distance_meter_deadzone",
2754
default_value="0.1",
2855
description="Deadzone in meters below which distance is not tracked.",
2956
),
30-
OpaqueFunction(function=launch_setup),
57+
DeclareLaunchArgument(
58+
"enable_rscp_hw_driver",
59+
default_value="false",
60+
description="Run the RSCP protocol driver. Only use it on real hardware.",
61+
),
62+
OpaqueFunction(function=launch_setup),
3163
]
3264
)

0 commit comments

Comments
 (0)