Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/ROS-209-process-hangs-on-failure…
Browse files Browse the repository at this point in the history
…-to-connect-to-lidar' into ROS-209-process-hangs-on-failure-to-connect-to-lidar-foxy
  • Loading branch information
Samahu committed Sep 12, 2023
2 parents 043d1a9 + 0cdafc8 commit 7748405
Show file tree
Hide file tree
Showing 5 changed files with 32 additions and 34 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.rst
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ Changelog
to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode
is used.
* fix: destagger columns timestamp when generating destaggered point clouds.
* shutdown the driver when unable to connect to the sensor on startup


ouster_ros v0.10.0
Expand Down
32 changes: 15 additions & 17 deletions ouster-ros/launch/driver.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
import launch
import lifecycle_msgs.msg
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node, LifecycleNode
from launch_ros.actions import LifecycleNode
from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
RegisterEventHandler, EmitEvent, LogInfo)
from launch.conditions import IfCondition
Expand Down Expand Up @@ -40,7 +40,8 @@ def generate_launch_description():
rviz_enable_arg = DeclareLaunchArgument('viz', default_value='True')

os_driver_name = LaunchConfiguration('os_driver_name')
os_driver_name_arg = DeclareLaunchArgument('os_driver_name', default_value='os_driver')
os_driver_name_arg = DeclareLaunchArgument(
'os_driver_name', default_value='os_driver')

os_driver = LifecycleNode(
package='ouster_ros',
Expand Down Expand Up @@ -72,20 +73,17 @@ def generate_launch_description():
)
)

# TODO: figure out why registering for on_shutdown event causes an exception
# and error handling
# shutdown_event = RegisterEventHandler(
# OnShutdown(
# on_shutdown=[
# EmitEvent(event=ChangeState(
# lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"),
# transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
# )),
# LogInfo(msg="os_sensor node exiting..."),
# ],
# )
# )

sensor_finalized_event = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=os_driver, goal_state='finalized',
entities=[
LogInfo(
msg="Failed to communicate with the sensor in a timely manner."),
EmitEvent(event=launch.events.Shutdown(
reason="Couldn't communicate with sensor"))
],
)
)

rviz_launch_file_path = \
Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py'
Expand All @@ -103,5 +101,5 @@ def generate_launch_description():
os_driver,
sensor_configure_event,
sensor_activate_event,
# shutdown_event
sensor_finalized_event
])
2 changes: 1 addition & 1 deletion ouster-ros/launch/sensor.composite.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
from pathlib import Path
import launch
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription,
ExecuteProcess, TimerAction)
Expand Down
29 changes: 14 additions & 15 deletions ouster-ros/launch/sensor.independent.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@ def generate_launch_description():
"""
ouster_ros_pkg_dir = get_package_share_directory('ouster_ros')
default_params_file = \
Path(ouster_ros_pkg_dir) / 'config' / 'os_sensor_cloud_image_params.yaml'
Path(ouster_ros_pkg_dir) / 'config' / \
'os_sensor_cloud_image_params.yaml'
params_file = LaunchConfiguration('params_file')
params_file_arg = DeclareLaunchArgument('params_file',
default_value=str(
Expand Down Expand Up @@ -69,19 +70,17 @@ def generate_launch_description():
)
)

# TODO: figure out why registering for on_shutdown event causes an exception
# and error handling
# shutdown_event = RegisterEventHandler(
# OnShutdown(
# on_shutdown=[
# EmitEvent(event=ChangeState(
# lifecycle_node_matcher=matches_node_name(node_name=F"/ouster/os_sensor"),
# transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN,
# )),
# LogInfo(msg="os_sensor node exiting..."),
# ],
# )
# )
sensor_finalized_event = RegisterEventHandler(
OnStateTransition(
target_lifecycle_node=os_sensor, goal_state='finalized',
entities=[
LogInfo(
msg="Failed to communicate with the sensor in a timely manner."),
EmitEvent(event=launch.events.Shutdown(
reason="Couldn't communicate with sensor"))
],
)
)

os_cloud = Node(
package='ouster_ros',
Expand Down Expand Up @@ -118,5 +117,5 @@ def generate_launch_description():
os_image,
sensor_configure_event,
sensor_activate_event,
# shutdown_event
sensor_finalized_event
])
2 changes: 1 addition & 1 deletion ouster-ros/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ouster_ros</name>
<version>0.10.2</version>
<version>0.10.3</version>
<description>Ouster ROS2 driver</description>
<maintainer email="[email protected]">ouster developers</maintainer>
<license file="LICENSE">BSD</license>
Expand Down

0 comments on commit 7748405

Please sign in to comment.