Skip to content

Commit a6c3395

Browse files
daisukest-hatakeyamyushi-minemura
authored
use rclpy instead of roslibpy (#31)
* import rclpy Signed-off-by: t-hatakeyam <[email protected]> * import rclpy Signed-off-by: t-hatakeyam <[email protected]> * import rclpy Signed-off-by: t-hatakeyam <[email protected]> * edit_Dockerfile Signed-off-by: t-hatakeyam <[email protected]> * publisher edit Signed-off-by: t-hatakeyam <[email protected]> * publisher edit Signed-off-by: t-hatakeyam <[email protected]> * publisher edit Signed-off-by: t-hatakeyam <[email protected]> * publisher Subscription edit Signed-off-by: t-hatakeyam <[email protected]> * error rclpy Signed-off-by: t-hatakeyam <[email protected]> * delete commentout Signed-off-by: t-hatakeyam <[email protected]> * thread error fix Signed-off-by: t-hatakeyam <[email protected]> * WIP: enable speak service to be executable Signed-off-by: yushi-minemura <[email protected]> * fix error caused by missing DBUS_SESSION_BUS_ADDRESS Signed-off-by: yushi-minemura <[email protected]> * fix topic name error Signed-off-by: yushi-minemura <[email protected]> * fix issue where exceptions were not being caught Signed-off-by: yushi-minemura <[email protected]> * fix typo Signed-off-by: yushi-minemura <[email protected]> * fix issue with unable to convert msg to dictionary Signed-off-by: yushi-minemura <[email protected]> * fix issues with diagnostic_agg_callback due to migration to ROS2 Signed-off-by: yushi-minemura <[email protected]> * fix issue where service was not returning response Signed-off-by: yushi-minemura <[email protected]> * fix issue where ros2 threads do not stop on Ctrl+C Signed-off-by: yushi-minemura <[email protected]> * change node name from cabot_node_* to cabot_app_server_* Signed-off-by: yushi-minemura <[email protected]> * set start_parameter_services=False when creating nodes Signed-off-by: yushi-minemura <[email protected]> * re-implement the code to call restart_localization service Signed-off-by: yushi-minemura <[email protected]> * delete unnecessary code Signed-off-by: yushi-minemura <[email protected]> * use multi-stage build to copy only necessary files from cabot-common and cabot-navigation packages Signed-off-by: yushi-minemura <[email protected]> * change the name of the dependency workspace Signed-off-by: yushi-minemura <[email protected]> * remove cabot-common submodule Signed-off-by: yushi-minemura <[email protected]> * remove leftover comments Signed-off-by: yushi-minemura <[email protected]> * explicitly destroy client instances for service Signed-off-by: yushi-minemura <[email protected]> * create service clients only during initialization Signed-off-by: yushi-minemura <[email protected]> * convert cabot_ace_battery_driver_node.py from roslibpy to rclpy Signed-off-by: Daisuke Sato <[email protected]> * update Dockerfile: build custom_ws in cache Signed-off-by: Daisuke Sato <[email protected]> * bugfix: byte value was converted as string Signed-off-by: Daisuke Sato <[email protected]> --------- Signed-off-by: t-hatakeyam <[email protected]> Signed-off-by: yushi-minemura <[email protected]> Signed-off-by: Daisuke Sato <[email protected]> Co-authored-by: t-hatakeyam <[email protected]> Co-authored-by: yushi-minemura <[email protected]>
1 parent 610a235 commit a6c3395

File tree

8 files changed

+226
-156
lines changed

8 files changed

+226
-156
lines changed

‎Dockerfile‎

Lines changed: 28 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,23 @@
1-
FROM ros:humble-ros-base
1+
FROM ros:humble-ros-base as cache
2+
3+
RUN apt update && \
4+
apt install -y --no-install-recommends \
5+
git \
6+
&& \
7+
apt clean && \
8+
rm -rf /var/lib/apt/lists/*
9+
10+
RUN cd /tmp && \
11+
git clone https://github.com/CMU-cabot/cabot-common.git && \
12+
git clone https://github.com/CMU-cabot/cabot-navigation.git && \
13+
mkdir -p /opt/custom_ws/src && \
14+
cd /opt/custom_ws && \
15+
cp -r /tmp/cabot-common/cabot_msgs src && \
16+
cp -r /tmp/cabot-navigation/mf_localization_msgs src && \
17+
. /opt/ros/humble/setup.sh && \
18+
colcon build
19+
20+
FROM ros:humble-ros-base as final
221

322
ENV DEBIAN_FRONTEND="noninteractive" \
423
UBUNTU_DISTRO=jammy
@@ -84,6 +103,8 @@ RUN apt update && \
84103
sudo \
85104
systemd \
86105
wireless-tools \
106+
ros-humble-rmw-cyclonedds-cpp \
107+
ros-humble-diagnostic-updater \
87108
&& \
88109
apt clean && \
89110
rm -rf /var/lib/apt/lists/*
@@ -94,6 +115,11 @@ COPY requirements.txt requirements.txt
94115
RUN pip3 install --no-cache-dir \
95116
-r requirements.txt
96117

118+
COPY --from=cache /opt/custom_ws/install /opt/custom_ws/install
119+
RUN sed -i 's:exec "$@":source /opt/custom_ws/install/setup.bash:' /ros_entrypoint.sh && \
120+
echo "export DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/\$(id -u)/bus" >> /ros_entrypoint.sh && \
121+
echo 'exec "$@"' >> /ros_entrypoint.sh
122+
97123
ARG USERNAME=developer
98124
ARG UID=1000
99125
RUN useradd -m $USERNAME && \
@@ -121,4 +147,4 @@ COPY entrypoint.sh /entrypoint.sh
121147
COPY cabot-device-check/check_device_status.sh $HOME/cabot-device-check/check_device_status.sh
122148
COPY cabot-device-check/locale $HOME/locale
123149

124-
ENTRYPOINT [ "/entrypoint.sh" ]
150+
ENTRYPOINT [ "/ros_entrypoint.sh" ]

‎ble.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -153,7 +153,7 @@ def handleSpeak(self, req, res):
153153
for ble in self.bles.values():
154154
if ble.speak_char:
155155
ble.speak_char.handleSpeak(req=req)
156-
res['result'] = True
156+
res.result = True
157157
return True
158158

159159
def handleEventCallback(self, msg, request_id):

‎cabot_ace/cabot_ace_battery_driver_node.py‎

Lines changed: 38 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,10 @@
33
import os
44
import logging
55
import time
6-
7-
import roslibpy
6+
import rclpy
7+
import rclpy.node
8+
from std_srvs.srv import Empty, SetBool
9+
import traceback
810

911
from .cabot_ace_battery_driver import BatteryDriver, BatteryDriverDelegate, BatteryStatus
1012
from cabot import util
@@ -28,87 +30,75 @@ def battery_status(self, status):
2830
logger.info("lowpower shutdown requested")
2931

3032

31-
class BatteryDriverNode:
32-
def __init__(self, client, driver):
33-
self.client = client
33+
class BatteryDriverNode(rclpy.node.Node):
34+
def __init__(self, driver):
35+
super().__init__("battery_driver_node", start_parameter_services=False)
3436
self.driver = driver
3537
self.connected = False
3638

37-
self.service0 = roslibpy.Service(self.client, '/ace_battery_control/turn_jetson_switch_on', 'std_srvs/Empty')
38-
self.service1 = roslibpy.Service(self.client, '/ace_battery_control/set_12v_power', 'std_srvs/SetBool')
39-
self.service2 = roslibpy.Service(self.client, '/ace_battery_control/set_5v_power', 'std_srvs/SetBool')
40-
self.service3 = roslibpy.Service(self.client, '/ace_battery_control/set_odrive_power', 'std_srvs/SetBool')
41-
self.service4 = roslibpy.Service(self.client, '/ace_battery_control/shutdown', 'std_srvs/Empty')
42-
#self.service5 = roslibpy.Service(self.client, '/ace_battery_control/set_lowpower_shutdown_threshold', 'cabot_msgs/SetUInt8')
43-
self.polling_ros()
44-
45-
@util.setInterval(1.0)
46-
def polling_ros(self):
47-
if not self.client.is_connected:
48-
self.connected = False
49-
else:
50-
if not self.connected:
51-
logger.info("advertise services")
52-
self.init_services()
53-
self.connected = True
54-
55-
def init_services(self):
56-
self.service0.advertise(self.turn_jetson_switch_on)
57-
self.service1.advertise(self.set_12v_power)
58-
self.service2.advertise(self.set_5v_power)
59-
self.service3.advertise(self.set_odrive_power)
60-
self.service4.advertise(self.shutdown)
61-
#self.service5.advertise(self.set_lowpower_shutdown_threshold)
39+
self.service0 = self.create_service(Empty, '/ace_battery_control/turn_jetson_switch_on', self.turn_jetson_switch_on)
40+
self.service1 = self.create_service(SetBool, '/ace_battery_control/set_12v_power', self.set_12v_power)
41+
self.service2 = self.create_service(SetBool, '/ace_battery_control/set_5v_power', self.set_5v_power)
42+
self.service3 = self.create_service(SetBool, '/ace_battery_control/set_odrive_power', self.set_odrive_power)
43+
self.service4 = self.create_service(Empty, '/ace_battery_control/shutdown', self.shutdown)
44+
#self.service5 = self.create_service(cabot_msgs.msg.SetUInt8, '/ace_battery_control/set_lowpower_shutdown_threshold', self.set_lowpower_shutdown_threshold)
45+
46+
def start(self):
47+
try:
48+
rclpy.spin(self)
49+
except:
50+
logger.error(traceback.format_exc())
6251

6352
def turn_jetson_switch_on(self, req, res):
6453
self.driver.turn_jetson_switch_on()
65-
return True
54+
return res
6655

6756
def set_12v_power(self, req, res):
68-
if req['data']:
57+
if req.data:
6958
self.driver.set_12v_power(1)
7059
else:
7160
self.driver.set_12v_power(0)
72-
res['success'] = True
73-
return True
61+
res.success = True
62+
return res
7463

7564
def set_5v_power(self, req, res):
76-
if req['data']:
65+
if req.data:
7766
self.driver.set_5v_power(1)
7867
else:
7968
self.driver.set_5v_power(0)
80-
res['success'] = True
81-
return True
69+
res.success = True
70+
return res
8271

8372
def set_odrive_power(self, req, res):
84-
if req['data']:
73+
if req.data:
8574
self.driver.set_odrive_power(1)
8675
else:
8776
self.driver.set_odrive_power(0)
88-
res['success'] = True
89-
return True
77+
res.success = True
78+
return res
9079

9180
def shutdown(self, req, res):
9281
self.driver.shutdown()
93-
return True
82+
return res
9483

9584
def set_lowpower_shutdown_threshold(self, req, res):
9685
self.driver.set_lowpower_shutdown_threshold(msg['value'])
97-
res['success'] = True
98-
return True
86+
return res
9987

10088
def main():
101-
client = roslibpy.Ros(host='localhost', port=9091)
89+
rclpy.init()
10290

10391
port_name = os.environ['CABOT_ACE_BATTERY_PORT'] if 'CABOT_ACE_BATTERY_PORT' in os.environ else '/dev/ttyACM0'
10492
baud = int(os.environ['CABOT_ACE_BATTERY_BAUD']) if 'CABOT_ACE_BATTERY_BAUD' in os.environ else 115200
10593
delegate = Delegate()
10694
driver = BatteryDriver(port_name, baud, delegate=delegate)
107-
BatteryDriverNode(client, driver)
108-
109-
client.run()
110-
driver.start()
95+
node = BatteryDriverNode(driver)
11196

97+
try:
98+
rclpy.spin(node)
99+
except:
100+
logger.error(traceback.format_exc())
101+
rclpy.destroy_node(node)
112102

113103
if __name__ == "__main__":
114104
logging.basicConfig(format='%(asctime)s - %(name)s - %(levelname)s - %(message)s')

‎cabot_app.py‎

Lines changed: 23 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,9 @@
2929
import signal
3030
import subprocess
3131
import sys
32+
from rosidl_runtime_py.convert import message_to_ordereddict
33+
34+
import rclpy
3235

3336
import common
3437
import ble
@@ -37,6 +40,7 @@
3740
from cabot import util
3841
from cabot_ace import BatteryDriverNode, BatteryDriver, BatteryDriverDelegate
3942
from cabot_log_report import LogReport
43+
from cabot_msgs.srv import Speak
4044

4145
MTU_SIZE = 2**10 # could be 2**15, but 2**10 is enough
4246
CHAR_WRITE_MAX_SIZE = 512 # should not be exceeded this value
@@ -297,8 +301,6 @@ def sigint_handler(sig, frame):
297301
battery_thread.join()
298302
if ble_manager:
299303
ble_manager.stop()
300-
common.client.terminate()
301-
common.cancel.set()
302304

303305
if tcp_server_thread:
304306
try:
@@ -308,6 +310,15 @@ def sigint_handler(sig, frame):
308310
while tcp_server_thread.is_alive():
309311
common.logger.info(f"wait tcp server thread {tcp_server_thread}")
310312
tcp_server_thread.join(timeout=1)
313+
314+
if common.ros2_thread:
315+
try:
316+
rclpy.shutdown()
317+
except:
318+
common.logger.error(traceback.format_exc())
319+
while common.ros2_thread.is_alive():
320+
common.logger.info(f"wait ros2 server thread {common.ros2_thread}")
321+
common.ros2_thread.join(timeout=1)
311322
except:
312323
common.logger.error(traceback.format_exc())
313324
else:
@@ -366,9 +377,11 @@ async def main():
366377
global battery_thread
367378
if port_name is not None and baud is not None:
368379
driver = BatteryDriver(port_name, baud, delegate=cabot_manager)
369-
battery_driver_node = BatteryDriverNode(common.client, driver)
380+
battery_driver_node = BatteryDriverNode(driver)
370381
battery_thread = threading.Thread(target=driver.start)
371382
battery_thread.start()
383+
battery_thread_node = threading.Thread(target=battery_driver_node.start)
384+
battery_thread_node.start()
372385

373386
result = subprocess.call(["grep", "-E", "^ControllerMode *= *le$", "/etc/bluetooth/main.conf"])
374387
if result != 0:
@@ -383,14 +396,16 @@ async def main():
383396
global quit_flag
384397

385398
def handleSpeak(req, res):
386-
req['request_id'] = time.clock_gettime_ns(time.CLOCK_REALTIME)
399+
res.result = False
400+
req_dictionary = message_to_ordereddict(req)
401+
req_dictionary['request_id'] = time.clock_gettime_ns(time.CLOCK_REALTIME)
387402
if ble_manager:
388-
ble_manager.handleSpeak(req, res)
403+
ble_manager.handleSpeak(req_dictionary, res)
389404
if tcp_server:
390-
tcp_server.handleSpeak(req, res)
391-
return True
405+
tcp_server.handleSpeak(req_dictionary, res)
406+
return res
392407

393-
common.speak_service.advertise(handleSpeak)
408+
common.cabot_node_common.create_service(Speak, '/speak', handleSpeak)
394409

395410
global tcp_server_thread
396411
try:

0 commit comments

Comments
 (0)