Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Event based reset #48

Open
wants to merge 3 commits into
base: intel_development
Choose a base branch
from
Open

Event based reset #48

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7eb2d0a
Check context for reset before attempting to acquire device
aaronplusone Jul 24, 2023
24f4693
Add c++17 flags since we build the sdk with the same
aaronplusone Jul 24, 2023
18e93ed
Add timeout for reset event wait
aaronplusone Jul 24, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions realsense2_camera/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.8.3)
project(realsense2_camera)
add_compile_options(-std=c++11)
add_compile_options(-std=c++17)

option(BUILD_WITH_OPENMP "Use OpenMP" OFF)
option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF)
Expand Down Expand Up @@ -62,7 +62,7 @@ endif()

if (WIN32)
else()
set(CMAKE_CXX_FLAGS "-fPIE -fPIC -std=c++11 -D_FORTIFY_SOURCE=2 -fstack-protector -Wformat -Wformat-security -Wall ${CMAKE_CXX_FLAGS}")
set(CMAKE_CXX_FLAGS "-fPIE -fPIC -std=c++17 -D_FORTIFY_SOURCE=2 -fstack-protector -Wformat -Wformat-security -Wall ${CMAKE_CXX_FLAGS}")
endif()

add_message_files(
Expand Down Expand Up @@ -128,7 +128,7 @@ target_link_libraries(${PROJECT_NAME}
if(WIN32)
set_target_properties(${realsense2_LIBRARY} PROPERTIES MAP_IMPORTED_CONFIG_RELWITHDEBINFO RELEASE)
target_link_libraries(${PROJECT_NAME}
realsense2::realsense2
realsense2::realsense2
realsense2::realsense-file
)
endif()
Expand Down Expand Up @@ -160,4 +160,3 @@ install(DIRECTORY rviz/
install(FILES nodelet_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

12 changes: 8 additions & 4 deletions realsense2_camera/include/realsense2_camera/constants.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#pragma once

#include <string>
#include <chrono>

#define REALSENSE_ROS_MAJOR_VERSION 2
#define REALSENSE_ROS_MINOR_VERSION 3
Expand Down Expand Up @@ -36,11 +37,11 @@ namespace realsense2_camera
const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB
const uint16_t RS405_PID = 0x0b0c; // DS5U
const uint16_t RS455_PID = 0x0B5C; // D455
const uint16_t RS_T265_PID = 0x0b37; //
const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; //
const uint16_t RS_L515_PID = 0x0B64; //
const uint16_t RS_T265_PID = 0x0b37; //
const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; //
const uint16_t RS_L515_PID = 0x0B64; //
const uint16_t RS_L535_PID = 0x0b68;


const bool ALIGN_DEPTH = false;
const bool POINTCLOUD = false;
Expand Down Expand Up @@ -117,4 +118,7 @@ namespace realsense2_camera
FISHEYE1, FISHEYE2, CONFIDENCE};

const std::vector<stream_index_pair> HID_STREAMS = {GYRO, ACCEL, POSE};

const std::chrono::duration<double> MAX_RESET_WAIT(10); // Max wait for reset event to complete in seconds
const std::chrono::seconds RESET_SLEEP_INTERVAL(1); // Sleep interval during reset check
} // namespace realsense2_camera
49 changes: 34 additions & 15 deletions realsense2_camera/src/realsense_node_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -132,7 +132,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
}
else
{
ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
ROS_INFO_STREAM("Device with port number " << port_id << " was found.");
}
bool found_device_type(true);
if (!_device_type.empty())
Expand Down Expand Up @@ -201,20 +201,39 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
_ctx.unload_tracking_module();
}

if (_device && _initial_reset)
{
_initial_reset = false;
try
{
ROS_INFO("Resetting device...");
_device.hardware_reset();
_device = rs2::device();
}
catch(const std::exception& ex)
{
ROS_WARN_STREAM("An exception has been thrown: " << ex.what());
}
}
if (_device && _initial_reset)
{
_initial_reset = false;
bool reset_complete(false);
try
{
rs2::context contex;
contex.set_devices_changed_callback([&](rs2::event_information& info)
{
// loop through all new devices
for (auto&& dev : info.get_new_devices())
{
auto dev_serial_no = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
reset_complete = dev_serial_no == _serial_no;
}
});


ROS_INFO("Resetting device...");
_device.hardware_reset();
auto start_time = std::chrono::steady_clock::now();
while(!reset_complete && std::chrono::steady_clock::now() - start_time < MAX_RESET_WAIT)
{
std::this_thread::sleep_for(RESET_SLEEP_INTERVAL);
}
ROS_INFO(reset_complete ? "Reset complete" : "Timed out waiting on reset event. Starting camera...");
_device = rs2::device();
}
catch(const std::exception& ex)
{
ROS_WARN_STREAM("An exception has been thrown: " << ex.what());
}
}
}

void RealSenseNodeFactory::change_device_callback(rs2::event_information& info)
Expand Down