Skip to content

Commit baf1917

Browse files
RafalS3RafalS3
authored
Merge pull request #113 from agh-space-systems-rover/rscp_module
2 parents 4343c78 + 85206fe commit baf1917

10 files changed

Lines changed: 908 additions & 0 deletions

File tree

‎kalman_arc/CMakeLists.txt‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,9 +82,18 @@ ament_target_dependencies(rscp_node
8282
kalman_interfaces
8383
)
8484

85+
add_executable(rscp_tcp_client src/rscp_tcp_client.cpp)
86+
87+
ament_target_dependencies(rscp_tcp_client
88+
rclcpp
89+
std_msgs
90+
)
91+
8592
add_library(rqt_rscp SHARED
8693
include/kalman_arc/rqt_rscp.hpp
94+
include/kalman_arc/rqt_rscp_over_tcp.hpp
8795
src/rqt_rscp.cpp
96+
src/rqt_rscp_over_tcp.cpp
8897
)
8998

9099
set_target_properties(rqt_rscp PROPERTIES AUTOMOC ON)
@@ -97,6 +106,8 @@ target_include_directories(rqt_rscp
97106

98107
target_link_libraries(rqt_rscp
99108
Qt5::Widgets
109+
kalman_arc_cobs
110+
rscp_proto_messages
100111
)
101112

102113
ament_target_dependencies(rqt_rscp
@@ -174,6 +185,7 @@ install(TARGETS
174185
kalman_arc_cobs
175186
rscp_proto_messages
176187
rscp_node
188+
rscp_tcp_client
177189
rqt_rscp
178190
darkest_rock_finder_node
179191
ARCHIVE DESTINATION lib

‎kalman_arc/README.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,3 +17,9 @@ rqt --standalone kalman_arc/RscpPublisher
1717

1818
It is also available in the regular rqt plugin menu under
1919
`Plugins > Kalman > RSCP Publisher`.
20+
21+
## For RSCP with raspi
22+
23+
``bash
24+
rqt --standalone kalman_arc/RscpOverTCP
25+
```

‎kalman_arc/include/kalman_arc/rqt_rscp_over_tcp.hpp‎

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

‎kalman_arc/launch/arc.launch.py‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,10 @@
55
OpaqueFunction,
66
)
77
from launch.substitutions import LaunchConfiguration
8+
from launch.actions import IncludeLaunchDescription
9+
from launch.launch_description_sources import PythonLaunchDescriptionSource
10+
from ament_index_python.packages import get_package_share_directory
11+
import os
812

913

1014
def launch_setup(context):
@@ -46,6 +50,19 @@ def get_bool(name):
4650
executable="rscp_node",
4751
),
4852
]
53+
arc_serial = IncludeLaunchDescription(
54+
PythonLaunchDescriptionSource(
55+
[
56+
os.path.join(get_package_share_directory("kalman_arc"), "launch"),
57+
"/arc_serial.launch.py",
58+
]
59+
)
60+
)
61+
62+
description += [
63+
arc_serial,
64+
]
65+
4966

5067
return description
5168

‎kalman_arc/launch/arc_serial.launch.py‎

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
from launch import LaunchDescription
2+
from launch.actions import (
3+
OpaqueFunction,
4+
EmitEvent,
5+
RegisterEventHandler,
6+
)
7+
from launch.substitutions import TextSubstitution
8+
from launch_ros.actions import LifecycleNode
9+
from launch.event_handlers import OnProcessStart
10+
from launch.event_handlers.on_shutdown import OnShutdown
11+
from launch.events import matches_action
12+
from launch_ros.event_handlers import OnStateTransition
13+
from launch_ros.events.lifecycle import ChangeState
14+
from lifecycle_msgs.msg import Transition
15+
16+
17+
def launch_setup(context):
18+
description = []
19+
serial_driver_onde = LifecycleNode(
20+
package='serial_driver',
21+
executable='serial_bridge',
22+
name='serial_bridge_node',
23+
namespace=TextSubstitution(text=''),
24+
parameters=[{'device_name': '/dev/ttyUSB0',
25+
'baud_rate': 115200,
26+
'flow_control': 'none',
27+
'parity': 'none',
28+
'stop_bits': "1"}],
29+
remappings=[("serial_read", "rscp/serial/rx"),
30+
("serial_write", "rscp/serial/tx")],
31+
output='screen',
32+
)
33+
configure_event_handler = RegisterEventHandler(
34+
event_handler=OnProcessStart(
35+
target_action=serial_driver_onde,
36+
on_start=[
37+
EmitEvent(
38+
event=ChangeState(
39+
lifecycle_node_matcher=matches_action(serial_driver_onde),
40+
transition_id=Transition.TRANSITION_CONFIGURE,
41+
),
42+
),
43+
],
44+
)
45+
)
46+
47+
activate_event_handler = RegisterEventHandler(
48+
event_handler=OnStateTransition(
49+
target_lifecycle_node=serial_driver_onde,
50+
start_state='configuring',
51+
goal_state='inactive',
52+
entities=[
53+
EmitEvent(
54+
event=ChangeState(
55+
lifecycle_node_matcher=matches_action(serial_driver_onde),
56+
transition_id=Transition.TRANSITION_ACTIVATE,
57+
),
58+
),
59+
],
60+
)
61+
)
62+
63+
shutdown_event_handler = RegisterEventHandler(
64+
event_handler=OnShutdown(
65+
on_shutdown=[
66+
EmitEvent(
67+
event=ChangeState(
68+
lifecycle_node_matcher=matches_action(serial_driver_onde),
69+
transition_id=Transition.TRANSITION_ACTIVE_SHUTDOWN,
70+
)
71+
)
72+
]
73+
)
74+
)
75+
description += [
76+
serial_driver_onde,
77+
configure_event_handler,
78+
activate_event_handler,
79+
shutdown_event_handler,
80+
]
81+
82+
83+
return description
84+
85+
86+
def generate_launch_description():
87+
return LaunchDescription(
88+
[
89+
OpaqueFunction(function=launch_setup),
90+
]
91+
)

‎kalman_arc/launch/rscp_tcp_client.launch.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
from launch import LaunchDescription
2+
from launch.actions import DeclareLaunchArgument
3+
from launch.substitutions import LaunchConfiguration
4+
from launch_ros.actions import Node
5+
from launch_ros.parameter_descriptions import ParameterValue
6+
7+
8+
def generate_launch_description():
9+
return LaunchDescription(
10+
[
11+
DeclareLaunchArgument(
12+
"ip_address",
13+
description="IP address of the RSCP TCP server.",
14+
),
15+
DeclareLaunchArgument(
16+
"port",
17+
default_value="5555",
18+
description="TCP port of the RSCP TCP server.",
19+
),
20+
Node(
21+
package="kalman_arc",
22+
executable="rscp_tcp_client",
23+
name="rscp_tcp_client",
24+
parameters=[
25+
{
26+
"host": LaunchConfiguration("ip_address"),
27+
"port": ParameterValue(
28+
LaunchConfiguration("port"),
29+
value_type=int,
30+
),
31+
}
32+
],
33+
output="screen",
34+
),
35+
]
36+
)

‎kalman_arc/plugin.xml‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,14 @@
99
<statustip>Publish RSCP request frames to rscp/serial/rx.</statustip>
1010
</qtgui>
1111
</class>
12+
<class name="kalman_arc/RscpOverTCP" type="kalman_arc::RqtRscpOverTcp" base_class_type="rqt_gui_cpp::Plugin">
13+
<description>Build RSCP request protobuf messages and publish COBS-framed bytes for the TCP bridge.</description>
14+
<qtgui>
15+
<group>
16+
<label>Kalman</label>
17+
</group>
18+
<label>RSCP Over TCP</label>
19+
<statustip>Publish and receive COBS-framed RSCP protobuf bytes over rscp/serial topics.</statustip>
20+
</qtgui>
21+
</class>
1222
</library>

0 commit comments

Comments
 (0)