diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index aac4a5070..803934756 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -57,12 +57,9 @@ RUN apt-get install -y \ ros-iron-camera-calibration \ ros-iron-desktop \ ros-iron-joint-state-publisher-gui \ - # ros-iron-plotjuggler-ros containing plotjuggler ros plugins - # build currently fails and is not available as a package so we - # have to manually install plotjuggler and plotjuggler-msgs - # https://github.com/PlotJuggler/plotjuggler-ros-plugins/issues/59 ros-iron-plotjuggler \ ros-iron-plotjuggler-msgs \ + ros-iron-plotjuggler-ros \ ros-iron-rmw-cyclonedds-cpp \ ros-iron-rqt-robot-monitor \ ros-iron-soccer-vision-3d-rviz-markers diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6b352bc39..7f7a39059 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -45,5 +45,5 @@ jobs: . /opt/ros/iron/setup.sh . install/setup.sh # Run tests for all packages - colcon test --packages-skip-regex pylon --event-handlers console_direct+ --return-code-on-test-failure --parallel-workers 1 + colcon test --event-handlers console_direct+ --return-code-on-test-failure --parallel-workers 1 working-directory: /colcon_ws diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index c08288577..2460329d7 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -16,7 +16,8 @@ "/root/colcon_ws/build/**/rosidl_generator_cpp/**", "${workspaceFolder}/**/include/**", "/opt/ros/${env:ROS_DISTRO}/include/**", - "/usr/include/**" + "/usr/include/**", + "/opt/pylon/include/**" ], "name": "ROS", "intelliSenseMode": "gcc-x64", diff --git a/.vscode/settings.json b/.vscode/settings.json index 7f3c780f6..4a2e3b67a 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -190,7 +190,9 @@ "typeindex": "cpp", "typeinfo": "cpp", "valarray": "cpp", - "variant": "cpp" + "variant": "cpp", + "regex": "cpp", + "future": "cpp" }, // Tell the ROS extension where to find the setup.bash // This also utilizes the COLCON_WS environment variable, which needs to be set diff --git a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt b/bitbots_misc/bitbots_basler_camera/CMakeLists.txt index e94d25096..bc2f79f7c 100644 --- a/bitbots_misc/bitbots_basler_camera/CMakeLists.txt +++ b/bitbots_misc/bitbots_basler_camera/CMakeLists.txt @@ -1,11 +1,50 @@ cmake_minimum_required(VERSION 3.5) project(bitbots_basler_camera) -find_package(bitbots_docs REQUIRED) +# Add support for C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +find_package(bitbots_docs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(OpenCV REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(pylon 7.1.0 REQUIRED) +find_package(camera_info_manager REQUIRED) +find_package(generate_parameter_library REQUIRED) + +add_compile_options(-Wall -Werror -Wno-unused) + +generate_parameter_library( + pylon_camera_parameters # cmake target name for the parameter library + config/camera_settings.yaml) + +add_executable(basler_camera src/basler_camera.cpp) + +target_link_libraries(basler_camera ${OpenCV_LIBRARIES} pylon::pylon + pylon_camera_parameters) + +ament_target_dependencies( + basler_camera + ament_cmake + bitbots_docs + cv_bridge + image_transport + rclcpp + sensor_msgs + camera_info_manager + generate_parameter_library + OpenCV) enable_bitbots_docs() +install(TARGETS basler_camera DESTINATION lib/${PROJECT_NAME}) + install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/bitbots_misc/bitbots_basler_camera/config/binning.yaml b/bitbots_misc/bitbots_basler_camera/config/binning.yaml deleted file mode 100644 index 7f43f9905..000000000 --- a/bitbots_misc/bitbots_basler_camera/config/binning.yaml +++ /dev/null @@ -1,3 +0,0 @@ -decimation_x: 4 -decimation_y: 4 -interpolation: 3 diff --git a/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml b/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml index 5f8f1a889..fc1d0c997 100644 --- a/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml +++ b/bitbots_misc/bitbots_basler_camera/config/camera_settings.yaml @@ -1,112 +1,69 @@ -pylon_camera_node: - ros__parameters: - # The tf frame under which the images were published - camera_frame: camera_optical_frame - - # The encoding of the pixels -- channel meaning, ordering, size - # taken from the list of strings in include/sensor_msgs/image_encodings.h - # The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', - # 'bayer_gbrg8' and 'bayer_rggb8' - # Default values are 'mono8' and 'rgb8' - image_encoding: "bayer_rggb8" - - # Binning factor to get downsampled images. It refers here to any camera - # setting which combines rectangular neighborhoods of pixels into larger - # "super-pixels." It reduces the resolution of the output image to - # (width / binning_x) x (height / binning_y). - # The default values binning_x = binning_y = 0 are considered the same - # as binning_x = binning_y = 1 (no subsampling). - # binning_x: 4 - # binning_y: 4 - - # The desired publisher frame rate if listening to the topics. - # This parameter can only be set once at startup - # Calling the GrabImages-Action can result in a higher framerate - frame_rate: 10.0 - - # Mode of camera's shutter. - # The supported modes are "rolling", "global" and "global_reset" - # Default value is "" (empty) means default_shutter_mode - shutter_mode: "global" - - ########################################################################## - ######################## Image Intensity Settings ######################## - ########################################################################## - # The following settings do *NOT* have to be set. Each camera has default - # values which provide an automatic image adjustment resulting in valid - # images - ########################################################################## - - # The exposure time in microseconds to be set after opening the camera. - exposure: 3000.0 - - # The target gain in percent of the maximal value the camera supports - # For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so - # called 'device specific units'. - gain: 0.55 - - # Gamma correction of pixel intensity. - # Adjusts the brightness of the pixel values output by the camera's sensor - # to account for a non-linearity in the human perception of brightness or - # of the display system (such as CRT). - gamma: 1.0 - - # The average intensity value of the images. It depends the exposure time - # as well as the gain setting. If 'exposure' is provided, the interface will - # try to reach the desired brightness by only varying the gain. (What may - # often fail, because the range of possible exposure vaules is many - # times higher than the gain range). If 'gain' is provided, the interface will - # try to reach the desired brightness by only varying the exposure time. If - # gain AND exposure are given, it is not possible to reach the brightness, - # because both are assumed to be fix. - # brightness: 100 - - # Only relevant, if 'brightness' is set: - # The brightness_continuous flag controls the auto brightness function. - # If it is set to false, the brightness will only be reached once. - # Hence changing light conditions lead to changing brightness values. - # If it is set to true, the given brightness will be reached continuously, - # trying to adapt to changing light conditions. This is only possible for - # values in the possible auto range of the pylon API which is e.g. [50 - 205] - # for acA2500-14um and acA1920-40gm - # brightness_continuous: true - - # Only relevant, if 'brightness' is set: - # If the camera should try to reach and / or keep the brightness, hence - # adapting to changing light conditions, at least one of the following flags - # must be set. - # If both are set, the interface will use the profile that tries to keep the - # gain at minimum to reduce white noise. - # The exposure_auto flag indicates, that the desired brightness will be - # reached by adapting the exposure time. - # The gain_auto flag indicates, that the desired brightness will be - # reached by adapting the gain. - # exposure_auto: true - # gain_auto: true - - ########################################################################## - - # The timeout while searching the exposure which is connected to the - # desired brightness. For slow system this has to be increased. - # exposure_search_timeout: 5.0 - - # The exposure search can be limited with an upper bound. This is to prevent - # very high exposure times and resulting timeouts. - # A typical value for this upper bound is ~2000000us. - # auto_exposure_upper_limit: 2000000.0 - - # The MTU size. Only used for GigE cameras. - # To prevent lost frames configure the camera has to be configured - # with the MTU size the network card supports. A value greater 3000 - # should be good (1500 for RaspberryPI) - # gige: - gige/mtu_size: 8000 - - # Only used for GigE cameras. - # The inter-package delay in ticks to prevent lost frames. - # For most of GigE-Cameras, a value of 1000 is reasonable. - # For cameras used on a RaspberryPI this value should be set to 11772. - # gige: - gige/inter_pkg_delay: 4000 - - rectification_active: false +pylon_camera_parameters: + exposure: + type: double + default_value: 3000.0 + description: "The exposure time in microseconds to be set after opening the camera." + validation: + bounds<>: [0.0, 1000000.0] + gain: + type: int + default_value: 200 + description: "The target raw gain of the camera sensor (similar to ISO)." + validation: + bounds<>: [0, 360] + fps: + type: double + default_value: 10.0 + read_only: true + description: "Target frame rate of the camera / publisher." + validation: + bounds<>: [0.0, 30.0] + binning_factor_x: + type: int + default_value: 4 + read_only: true + description: "Binning factor to get downsampled images in x direction." + validation: + gt_eq<>: [1] + binning_factor_y: + type: int + default_value: 4 + read_only: true + description: "Binning factor to get downsampled images in y direction." + validation: + gt_eq<>: [1] + camera_info_url: + type: string + default_value: "" + read_only: true + description: "The URL of the camera calibration file." + validation: + not_empty<>: [] + device_user_id: + type: string + default_value: "" + read_only: true + description: "The name of the camera (used to discover the camera). The name can be set in the pylon viewer." + validation: + not_empty<>: [] + camera_frame_id: + type: string + default_value: "camera_optical_frame" + read_only: true + description: "The tf frame at which the camera's optical center is located." + validation: + not_empty<>: [] + gige: + mtu_size: + type: int + default_value: 9000 + description: "The MTU size for GigE cameras." + read_only: true + + inter_pkg_delay: + type: int + default_value: 1000 + description: "The inter-package delay in 'ticks' for GigE cameras." + read_only: true + validation: + bounds<>: [0, 10000] diff --git a/bitbots_misc/bitbots_basler_camera/config/default.yaml b/bitbots_misc/bitbots_basler_camera/config/default.yaml deleted file mode 100644 index d0c85ac72..000000000 --- a/bitbots_misc/bitbots_basler_camera/config/default.yaml +++ /dev/null @@ -1,123 +0,0 @@ -pylon_camera_node: - ros__parameters: - # The tf frame under which the images were published - camera_frame: pylon_camera - - # The DeviceUserID of the camera. If empty, the first camera found in the - # device list will be used - device_user_id: "" - - # The CameraInfo URL (Uniform Resource Locator) where the optional intrinsic - # camera calibration parameters are stored. This URL string will be parsed - # from the ROS-CameraInfoManager: - # http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_ - # 1_1CameraInfoManager.html#details - camera_info_url: "" - - # The encoding of the pixels -- channel meaning, ordering, size - # taken from the list of strings in include/sensor_msgs/image_encodings.h - # The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', - # 'bayer_gbrg8' and 'bayer_rggb8' - # Default values are 'mono8' and 'rgb8' - image_encoding: "bayer_rggb8" - - # Binning factor to get downsampled images. It refers here to any camera - # setting which combines rectangular neighborhoods of pixels into larger - # "super-pixels." It reduces the resolution of the output image to - # (width / binning_x) x (height / binning_y). - # The default values binning_x = binning_y = 0 are considered the same - # as binning_x = binning_y = 1 (no subsampling). - # binning_x: 4 - # binning_y: 4 - - # The desired publisher frame rate if listening to the topics. - # This parameter can only be set once at startup - # Calling the GrabImages-Action can result in a higher framerate - frame_rate: 20.0 - - # Mode of camera's shutter. - # The supported modes are "rolling", "global" and "global_reset" - # Default value is "" (empty) means default_shutter_mode - shutter_mode: "global" - - ########################################################################## - ######################## Image Intensity Settings ######################## - ########################################################################## - # The following settings do *NOT* have to be set. Each camera has default - # values which provide an automatic image adjustment resulting in valid - # images - ########################################################################## - - # The exposure time in microseconds to be set after opening the camera. - exposure: 2000.0 - - # The target gain in percent of the maximal value the camera supports - # For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so - # called 'device specific units'. - gain: 0.4 - - # Gamma correction of pixel intensity. - # Adjusts the brightness of the pixel values output by the camera's sensor - # to account for a non-linearity in the human perception of brightness or - # of the display system (such as CRT). - # gamma: 1.0 - - # The average intensity value of the images. It depends the exposure time - # as well as the gain setting. If 'exposure' is provided, the interface will - # try to reach the desired brightness by only varying the gain. (What may - # often fail, because the range of possible exposure vaules is many - # times higher than the gain range). If 'gain' is provided, the interface will - # try to reach the desired brightness by only varying the exposure time. If - # gain AND exposure are given, it is not possible to reach the brightness, - # because both are assumed to be fix. - # brightness: 100 - - # Only relevant, if 'brightness' is set: - # The brightness_continuous flag controls the auto brightness function. - # If it is set to false, the brightness will only be reached once. - # Hence changing light conditions lead to changing brightness values. - # If it is set to true, the given brightness will be reached continuously, - # trying to adapt to changing light conditions. This is only possible for - # values in the possible auto range of the pylon API which is e.g. [50 - 205] - # for acA2500-14um and acA1920-40gm - # brightness_continuous: true - - # Only relevant, if 'brightness' is set: - # If the camera should try to reach and / or keep the brightness, hence - # adapting to changing light conditions, at least one of the following flags - # must be set. - # If both are set, the interface will use the profile that tries to keep the - # gain at minimum to reduce white noise. - # The exposure_auto flag indicates, that the desired brightness will be - # reached by adapting the exposure time. - # The gain_auto flag indicates, that the desired brightness will be - # reached by adapting the gain. - # exposure_auto: true - # gain_auto: true - - ########################################################################## - - # The timeout while searching the exposure which is connected to the - # desired brightness. For slow system this has to be increased. - # exposure_search_timeout: 5.0 - - # The exposure search can be limited with an upper bound. This is to prevent - # very high exposure times and resulting timeouts. - # A typical value for this upper bound is ~2000000us. - # auto_exposure_upper_limit: 2000000.0 - - # The MTU size. Only used for GigE cameras. - # To prevent lost frames configure the camera has to be configured - # with the MTU size the network card supports. A value greater 3000 - # should be good (1500 for RaspberryPI) - # gige: - gige/mtu_size: 1500 - - # Only used for GigE cameras. - # The inter-package delay in ticks to prevent lost frames. - # For most of GigE-Cameras, a value of 1000 is reasonable. - # For cameras used on a RaspberryPI this value should be set to 11772. - # gige: - #inter_pkg_delay: 131072 - - reactification_active: false diff --git a/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch b/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch index fc9a6a203..56263a669 100644 --- a/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch +++ b/bitbots_misc/bitbots_basler_camera/launch/basler_camera.launch @@ -3,34 +3,8 @@ - - - + - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/bitbots_misc/bitbots_basler_camera/package.xml b/bitbots_misc/bitbots_basler_camera/package.xml index b08289f66..88508bfbe 100644 --- a/bitbots_misc/bitbots_basler_camera/package.xml +++ b/bitbots_misc/bitbots_basler_camera/package.xml @@ -3,7 +3,7 @@ bitbots_basler_camera 1.0.0 - This sets up the basler camera + This contains the interface between the Basler camera's pylon SDK and ROS 2. In addtion to that is also applies some postprocessing (debayering, downsampling) to the images. Marc Bestmann Hamburg Bit-Bots @@ -13,8 +13,13 @@ Hamburg Bit-Bots bitbots_docs - pylon_ros2_camera_wrapper - image_proc + cv_bridge + image_transport + libopencv-dev + rclcpp + sensor_msgs + camera_info_manager + generate_parameter_library ament_cmake diff --git a/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp new file mode 100644 index 000000000..009c87cd1 --- /dev/null +++ b/bitbots_misc/bitbots_basler_camera/src/basler_camera.cpp @@ -0,0 +1,243 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "pylon_camera_parameters.hpp" + +using std::placeholders::_1, std::placeholders::_2; +using namespace Pylon; + +namespace basler_camera { + +class PostProcessor { + std::shared_ptr node_; + + image_transport::TransportHints transport_hints_; + std::unique_ptr image_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr camera_info_manager_; + + std::unique_ptr camera_; + + double camera_tick_frequency_ = 1; + + Pylon::PylonAutoInitTerm autoInitTerm; + + std::unique_ptr param_listener_; + pylon_camera_parameters::Params config_; + + public: + PostProcessor() + : node_(std::make_shared("post_processor")), + transport_hints_(node_.get()), + image_pub_(std::make_unique( + image_transport::create_camera_publisher(node_.get(), "camera/image_proc"))) { + // Load parameters + param_listener_ = std::make_unique(node_); + config_ = param_listener_->get_params(); + + // Set up camera info manager + camera_info_manager_ = std::make_unique(node_.get(), config_.device_user_id, + config_.camera_info_url); + + try { + // Initialize the camera + initilize_camera(); + + } catch (GenICam::GenericException& e) { + // Error handling. + RCLCPP_ERROR(node_->get_logger(), "An exception occurred: %s", e.GetDescription()); + RCLCPP_ERROR(node_->get_logger(), "Could not initialize camera"); + exit(1); + } + + // Setup timer for publishing + timer_ = node_->create_wall_timer(std::chrono::duration(1.0 / config_.fps), + std::bind(&PostProcessor::timer_callback, this)); + } + + void initilize_camera() { + // List all available devices + Pylon::DeviceInfoList_t devices; + CTlFactory::GetInstance().EnumerateDevices(devices); + + // The device user id of the camera we want to use + std::optional our_device_info; + + // Print all devices and their names + for (auto device : devices) { + RCLCPP_INFO(node_->get_logger(), "Device: %s (%s)", device.GetFriendlyName().c_str(), + device.GetModelName().c_str()); + // Check if the device has a user defined name and if it is the one we want to use + if (device.GetUserDefinedName().compare(config_.device_user_id.c_str()) == 0) { + our_device_info = device; + } + } + + // Check if the device was found + if (!our_device_info) { + RCLCPP_ERROR(node_->get_logger(), "Could not find device with user id '%s'", config_.device_user_id.c_str()); + return; + } else { + RCLCPP_INFO(node_->get_logger(), "Using device user id '%s'", config_.device_user_id.c_str()); + } + + // Set up the camera + camera_ = std::make_unique( + CTlFactory::GetInstance().CreateDevice(our_device_info.value())); + + // Wait for the camera to be ready + camera_->Open(); + + // Set MTU and inter-package delay + camera_->GevSCPSPacketSize.SetValue(config_.gige.mtu_size); + camera_->GevSCPD.SetValue(config_.gige.inter_pkg_delay); + + camera_->ClearBufferModeEnable(); + + // Set the camera parameters + apply_camera_parameters(); + + // Set static camera parameters + camera_->ShutterMode.SetValue(Basler_UniversalCameraParams::ShutterModeEnums::ShutterMode_Global); + camera_->Width.SetToMaximum(); + camera_->Height.SetToMaximum(); + camera_->PixelFormat.SetValue(Basler_UniversalCameraParams::PixelFormat_BayerRG8); + camera_->BalanceWhiteAuto.SetValue( + Basler_UniversalCameraParams::BalanceWhiteAutoEnums::BalanceWhiteAuto_Continuous); + + // Set to manual acquisition mode + camera_->AcquisitionMode.SetValue(Basler_UniversalCameraParams::AcquisitionModeEnums::AcquisitionMode_Continuous); + + // Get the camera frequency + camera_tick_frequency_ = camera_->GevTimestampTickFrequency(); + + // Start grabbing + camera_->StartGrabbing(GrabStrategy_LatestImageOnly); + } + + void apply_camera_parameters() { + // Set the camera parameters + camera_->GainAuto.SetValue(Basler_UniversalCameraParams::GainAutoEnums::GainAuto_Off); + camera_->ExposureAuto.SetValue(Basler_UniversalCameraParams::ExposureAutoEnums::ExposureAuto_Off); + camera_->GainRaw.SetValue(config_.gain); + camera_->ExposureTimeAbs.SetValue(config_.exposure); + } + + void timer_callback() { + // This smart pointer will receive the grab result data. + CGrabResultPtr ptrGrabResult; + + // Field to store the trigger time + rclcpp::Time trigger_time; + + try { + // Check if the config has changed + if (param_listener_->is_old(config_)) { + // Update the camera parameters + config_ = param_listener_->get_params(); + // Apply the new camera parameters + apply_camera_parameters(); + } + + // Try to reinitialize the camera if the connection is lost + if (!camera_->IsOpen() or camera_->IsCameraDeviceRemoved()) { + RCLCPP_WARN(node_->get_logger(), "Camera connection lost. Reinitializing camera"); + initilize_camera(); + } + + // Get current camera time + camera_->GevTimestampControlLatch(); + auto camera_time_stamp_at_capture = (__int128_t)camera_->GevTimestampValue(); + camera_->GevTimestampControlLatchReset(); + auto ros_time_stamp_at_capture = node_->now(); + + // Start frame acquisition + camera_->ExecuteSoftwareTrigger(); + + // Wait for an image and then retrieve it. A timeout of 5000 ms is used. + camera_->RetrieveResult(100, ptrGrabResult, TimeoutHandling_ThrowException); + + // Image grabbed successfully? + if (!ptrGrabResult->GrabSucceeded()) { + RCLCPP_ERROR(node_->get_logger(), "Error: %x %s", ptrGrabResult->GetErrorCode(), + ptrGrabResult->GetErrorDescription().c_str()); + return; + } + + // Adjust the time stamp of the image + auto image_camera_stamp = (__int128_t)ptrGrabResult->GetTimeStamp(); + auto image_age_in_seconds = (image_camera_stamp - camera_time_stamp_at_capture) / camera_tick_frequency_; + trigger_time = ros_time_stamp_at_capture + rclcpp::Duration::from_seconds(image_age_in_seconds); + } catch (GenICam::GenericException& e) { + // Error handling. + RCLCPP_ERROR(node_->get_logger(), "An exception occurred: %s", e.GetDescription()); + return; + } + + // Convert to cv::Mat + cv::Mat image(ptrGrabResult->GetHeight(), ptrGrabResult->GetWidth(), CV_8UC1, (uint8_t*)ptrGrabResult->GetBuffer()); + + // Create cv::Mat for color image + cv::Mat color(image.size(), CV_MAKETYPE(CV_8U, 3)); + + // Debayer the image + cv::cvtColor(image, color, cv::COLOR_BayerBG2BGR); + + // Perform binning by a given factor + cv::Mat binned(image.size().height / config_.binning_factor_y, image.size().width / config_.binning_factor_x, + CV_MAKETYPE(CV_8U, 3)); + cv::resize(color, binned, cv::Size(), 1.0 / config_.binning_factor_x, 1.0 / config_.binning_factor_y, + cv::INTER_AREA); + + // Add the binning to the camera info + auto camera_info = std::make_shared(camera_info_manager_->getCameraInfo()); + camera_info->binning_x = config_.binning_factor_x; + camera_info->binning_y = config_.binning_factor_y; + camera_info->header.frame_id = config_.camera_frame_id; + camera_info->header.stamp = trigger_time; + + // Convert back to sensor_msgs::Image + cv_bridge::CvImage color_msg; + color_msg.header = camera_info->header; + color_msg.encoding = sensor_msgs::image_encodings::BGR8; + color_msg.image = binned; + + // Publish the image + image_pub_->publish(color_msg.toImageMsg(), camera_info); + } + + /** + * @brief A getter that returns the node + */ + std::shared_ptr get_node() { return node_; } +}; +} // namespace basler_camera + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::experimental::executors::EventsExecutor exec = rclcpp::experimental::executors::EventsExecutor( + std::make_unique(), true, rclcpp::ExecutorOptions()); + + auto post_processor = std::make_shared(); + exec.add_node(post_processor->get_node()); + exec.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/bitbots_misc/bitbots_ceiling_cam/config/camera_settings_ceiling_cam.yaml b/bitbots_misc/bitbots_ceiling_cam/config/camera_settings_ceiling_cam.yaml index 424b70819..9abff1c91 100644 --- a/bitbots_misc/bitbots_ceiling_cam/config/camera_settings_ceiling_cam.yaml +++ b/bitbots_misc/bitbots_ceiling_cam/config/camera_settings_ceiling_cam.yaml @@ -1,123 +1,28 @@ -/**: +/ceiling_cam_publisher: ros__parameters: - - # The tf frame under which the images were published - camera_frame: ceiling_cam - # The DeviceUserID of the camera. If empty, the first camera found in the - # device list will be used - # device_user_id: "" + # The tf frame under which the images were published + camera_frame_id: ceiling_cam - # The CameraInfo URL (Uniform Resource Locator) where the optional intrinsic - # camera calibration parameters are stored. This URL string will be parsed - # from the ROS-CameraInfoManager: - # http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_ - # 1_1CameraInfoManager.html#details - camera_info_url: "package://bitbots_ceiling_cam/config/camera_calibration_ceiling_cam.yaml" - - # The encoding of the pixels -- channel meaning, ordering, size - # taken from the list of strings in include/sensor_msgs/image_encodings.h - # The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', - # 'bayer_gbrg8' and 'bayer_rggb8' - # Default values are 'mono8' and 'rgb8' - image_encoding: "bayer_rggb8" + # The name of the camera (used to discover the camera). The name can be set in the pylon viewer. + device_user_id: ceiling_cam - # Binning factor to get downsampled images. It refers here to any camera - # setting which combines rectangular neighborhoods of pixels into larger - # "super-pixels." It reduces the resolution of the output image to - # (width / binning_x) x (height / binning_y). - # The default values binning_x = binning_y = 0 are considered the same - # as binning_x = binning_y = 1 (no subsampling). - # binning_x: 4 - # binning_y: 4 - - # The desired publisher frame rate if listening to the topics. - # This parameter can only be set once at startup - # Calling the GrabImages-Action can result in a higher framerate - frame_rate: 20.0 + # The CameraInfo URL (Uniform Resource Locator) where the optional intrinsic + # camera calibration parameters are stored. This URL string will be parsed + # from the ROS-CameraInfoManager: + # http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_1_1CameraInfoManager.html#details + camera_info_url: "package://bitbots_ceiling_cam/config/camera_calibration_ceiling_cam.yaml" - # Mode of camera's shutter. - # The supported modes are "rolling", "global" and "global_reset" - # Default value is "" (empty) means default_shutter_mode - shutter_mode: "global" + # No subsampling is used for the ceiling camera + binning_factor_x: 1 + binning_factor_y: 1 - ########################################################################## - ######################## Image Intensity Settings ######################## - ########################################################################## - # The following settings do *NOT* have to be set. Each camera has default - # values which provide an automatic image adjustment resulting in valid - # images - ########################################################################## + # The target frame rate + fps: 20.0 - # The exposure time in microseconds to be set after opening the camera. + # The exposure time in microseconds exposure: 8000.0 - # The target gain in percent of the maximal value the camera supports - # For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so - # called 'device specific units'. - gain: 0.3 - - # Gamma correction of pixel intensity. - # Adjusts the brightness of the pixel values output by the camera's sensor - # to account for a non-linearity in the human perception of brightness or - # of the display system (such as CRT). - gamma: 1.0 - - # The average intensity value of the images. It depends the exposure time - # as well as the gain setting. If 'exposure' is provided, the interface will - # try to reach the desired brightness by only varying the gain. (What may - # often fail, because the range of possible exposure vaules is many - # times higher than the gain range). If 'gain' is provided, the interface will - # try to reach the desired brightness by only varying the exposure time. If - # gain AND exposure are given, it is not possible to reach the brightness, - # because both are assumed to be fix. - # brightness: 100 - - # Only relevant, if 'brightness' is set: - # The brightness_continuous flag controls the auto brightness function. - # If it is set to false, the brightness will only be reached once. - # Hence changing light conditions lead to changing brightness values. - # If it is set to true, the given brightness will be reached continuously, - # trying to adapt to changing light conditions. This is only possible for - # values in the possible auto range of the pylon API which is e.g. [50 - 205] - # for acA2500-14um and acA1920-40gm - # brightness_continuous: true - - # Only relevant, if 'brightness' is set: - # If the camera should try to reach and / or keep the brightness, hence - # adapting to changing light conditions, at least one of the following flags - # must be set. - # If both are set, the interface will use the profile that tries to keep the - # gain at minimum to reduce white noise. - # The exposure_auto flag indicates, that the desired brightness will be - # reached by adapting the exposure time. - # The gain_auto flag indicates, that the desired brightness will be - # reached by adapting the gain. - # exposure_auto: true - # gain_auto: true - - ########################################################################## - - # The timeout while searching the exposure which is connected to the - # desired brightness. For slow system this has to be increased. - # exposure_search_timeout: 5.0 - - # The exposure search can be limited with an upper bound. This is to prevent - # very high exposure times and resulting timeouts. - # A typical value for this upper bound is ~2000000us. - # auto_exposure_upper_limit: 2000000.0 - - # The MTU size. Only used for GigE cameras. - # To prevent lost frames configure the camera has to be configured - # with the MTU size the network card supports. A value greater 3000 - # should be good (1500 for RaspberryPI) - # gige: - # gige/mtu_size: 9000 - - # Only used for GigE cameras. - # The inter-package delay in ticks to prevent lost frames. - # For most of GigE-Cameras, a value of 1000 is reasonable. - # For cameras used on a RaspberryPI this value should be set to 11772. - # gige: - # gige/inter_pkg_delay: 1000 + # The target raw gain of the camera sensor (similar to ISO) + gain: 150 diff --git a/bitbots_misc/bitbots_ceiling_cam/launch/ceiling_cam.launch b/bitbots_misc/bitbots_ceiling_cam/launch/ceiling_cam.launch index 1a3ef6733..de3c237db 100644 --- a/bitbots_misc/bitbots_ceiling_cam/launch/ceiling_cam.launch +++ b/bitbots_misc/bitbots_ceiling_cam/launch/ceiling_cam.launch @@ -3,29 +3,23 @@ - - - - - + + + + + + - - - - - - + - + - + - - diff --git a/bitbots_misc/bitbots_ceiling_cam/package.xml b/bitbots_misc/bitbots_ceiling_cam/package.xml index cf140617e..18657f90c 100644 --- a/bitbots_misc/bitbots_ceiling_cam/package.xml +++ b/bitbots_misc/bitbots_ceiling_cam/package.xml @@ -7,13 +7,14 @@ Marc Bestmann Hamburg Bit-Bots - + MIT Hamburg Bit-Bots + apriltag_ros + bitbots_basler_camera bitbots_docs - pylon_ros2_camera_wrapper image_proc tf2_ros diff --git a/bitbots_misc/bitbots_containers/hlvs/Dockerfile b/bitbots_misc/bitbots_containers/hlvs/Dockerfile index 2affe1c26..71ef58d4c 100644 --- a/bitbots_misc/bitbots_containers/hlvs/Dockerfile +++ b/bitbots_misc/bitbots_containers/hlvs/Dockerfile @@ -63,7 +63,7 @@ RUN cd src/bitbots_main && \ rm -rf lib/udp_bridge bitbots_misc/bitbots_containers \ lib/dynamic_stack_decider/dynamic_stack_decider_visualization bitbots_lowlevel \ bitbots_wolfgang/wolfgang_pybullet_sim lib/DynamixelSDK lib/dynamixel-workbench \ - bitbots_misc/bitbots_basler_camera lib/pylon-ros-camera && \ + bitbots_misc/bitbots_basler_camera && \ sed -i '/plotjuggler/d' bitbots_motion/bitbots_quintic_walk/package.xml && \ sed -i '/run_depend/d' bitbots_wolfgang/wolfgang_moveit_config/package.xml diff --git a/bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt b/bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt index 1dfc02d76..f08aaeb3d 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt +++ b/bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt @@ -17,8 +17,6 @@ find_package(tf2 REQUIRED) find_package(rot_conv REQUIRED) find_package(backward_ros REQUIRED) -include_directories(include) - add_compile_options(-Wall -Werror -Wno-unused) add_executable(extrinsic_calibration src/extrinsic_calibration.cpp) diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml index 5944a576d..989323e24 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml @@ -1,5 +1,11 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: ros__parameters: - offset_x: 0.0 + offset_x: -0.03 offset_y: 0.0 offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: + ros__parameters: + offset_x: -0.03 + offset_y: -0.08 + offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/default.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/default.yaml index 5944a576d..93bdfd39d 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/default.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/default.yaml @@ -1,4 +1,10 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: + ros__parameters: + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: ros__parameters: offset_x: 0.0 offset_y: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml index 5944a576d..93bdfd39d 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/donna.yaml @@ -1,4 +1,10 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: + ros__parameters: + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: ros__parameters: offset_x: 0.0 offset_y: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml index ed0cbe9f8..60fd7357c 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/jack.yaml @@ -1,5 +1,11 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: ros__parameters: offset_x: -0.05 offset_y: 0.15 offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: + ros__parameters: + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/melody.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/melody.yaml index 5944a576d..93bdfd39d 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/melody.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/melody.yaml @@ -1,4 +1,10 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: + ros__parameters: + offset_x: 0.0 + offset_y: 0.0 + offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: ros__parameters: offset_x: 0.0 offset_y: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml b/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml index 5944a576d..fccb85d09 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml +++ b/bitbots_misc/bitbots_extrinsic_calibration/config/rory.yaml @@ -1,5 +1,11 @@ -/bitbots_extrinsic_calibration: +/bitbots_extrinsic_camera_calibration: ros__parameters: - offset_x: 0.0 + offset_x: -0.03 offset_y: 0.0 offset_z: 0.0 + +/bitbots_extrinsic_imu_calibration: + ros__parameters: + offset_x: -0.03 + offset_y: 0.03 + offset_z: 0.0 diff --git a/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp b/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp deleted file mode 100644 index 610cfd063..000000000 --- a/bitbots_misc/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -using std::placeholders::_1; - -class ExtrinsicCalibrationBroadcaster : public rclcpp::Node { - public: - ExtrinsicCalibrationBroadcaster(); - void step(); - - private: - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - std::unique_ptr broadcaster_; - geometry_msgs::msg::Transform transform_{geometry_msgs::msg::Transform()}; - std::string parent_frame_, child_frame_; - double offset_x_ = 0, offset_y_ = 0, offset_z_ = 0; - rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters); -}; diff --git a/bitbots_misc/bitbots_extrinsic_calibration/launch/calibration.launch b/bitbots_misc/bitbots_extrinsic_calibration/launch/calibration.launch index d4b136cb4..6e4e4c08d 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/launch/calibration.launch +++ b/bitbots_misc/bitbots_extrinsic_calibration/launch/calibration.launch @@ -1,8 +1,16 @@ - + + + + + + + + + diff --git a/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp b/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp index 82342cf27..1530622d4 100644 --- a/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp +++ b/bitbots_misc/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp @@ -1,74 +1,82 @@ -#include - -ExtrinsicCalibrationBroadcaster::ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") { - broadcaster_ = std::make_unique(this); - - this->declare_parameter("parent_frame", "camera"); - this->declare_parameter("child_frame", "camera_optical_frame"); - this->declare_parameter("offset_x", 0.0, rcl_interfaces::msg::ParameterDescriptor()); - this->declare_parameter("offset_y", 0.0); - this->declare_parameter("offset_z", 0.0); - - auto parameters = this->get_parameters(this->list_parameters({}, 10).names); - ExtrinsicCalibrationBroadcaster::onSetParameters(parameters); - - param_callback_handle_ = - this->add_on_set_parameters_callback(std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1)); -} - -rcl_interfaces::msg::SetParametersResult ExtrinsicCalibrationBroadcaster::onSetParameters( - const std::vector ¶meters) { - for (const auto ¶meter : parameters) { - if (parameter.get_name() == "offset_x") { - offset_x_ = parameter.as_double(); - } else if (parameter.get_name() == "offset_y") { - offset_y_ = parameter.as_double(); - } else if (parameter.get_name() == "offset_z") { - offset_z_ = parameter.as_double(); - } else if (parameter.get_name() == "parent_frame") { - parent_frame_ = parameter.as_string(); - } else if (parameter.get_name() == "child_frame") { - child_frame_ = parameter.as_string(); - } else { - RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str()); - } +#include +#include +#include +#include + +#include +#include +#include +#include +#include +using std::placeholders::_1; + +class ExtrinsicCalibrationBroadcaster : public rclcpp::Node { + public: + ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") { + broadcaster_ = std::make_unique(this); + + this->declare_parameter("parent_frame", "camera_optical_frame_uncalibrated"); + this->declare_parameter("child_frame", "camera_optical_frame"); + this->declare_parameter("offset_x", 0.0); + this->declare_parameter("offset_y", 0.0); + this->declare_parameter("offset_z", 0.0); + + auto parameters = this->get_parameters(this->list_parameters({}, 10).names); + onSetParameters(parameters); + + param_callback_handle_ = + this->add_on_set_parameters_callback(std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1)); } - auto base_quat = rot_conv::QuatFromEuler(-1.5708, 0.0, -1.5708); - auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_); - - auto final_quat = offset_quat * base_quat; + rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector ¶meters) { + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "offset_x") { + offset_x_ = parameter.as_double(); + } else if (parameter.get_name() == "offset_y") { + offset_y_ = parameter.as_double(); + } else if (parameter.get_name() == "offset_z") { + offset_z_ = parameter.as_double(); + } else if (parameter.get_name() == "parent_frame") { + parent_frame_ = parameter.as_string(); + } else if (parameter.get_name() == "child_frame") { + child_frame_ = parameter.as_string(); + } else { + RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str()); + } + } + auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_); - transform_.rotation.x = final_quat.x(); - transform_.rotation.y = final_quat.y(); - transform_.rotation.z = final_quat.z(); - transform_.rotation.w = final_quat.w(); + transform_.rotation.x = offset_quat.x(); + transform_.rotation.y = offset_quat.y(); + transform_.rotation.z = offset_quat.z(); + transform_.rotation.w = offset_quat.w(); - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = this->now(); + tf.header.frame_id = parent_frame_; + tf.child_frame_id = child_frame_; + tf.transform = transform_; + broadcaster_->sendTransform(tf); - return result; -} + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; -void ExtrinsicCalibrationBroadcaster::step() { - rclcpp::Time now = this->now(); + return result; + } - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent_frame_; - tf.child_frame_id = child_frame_; - tf.transform = transform_; - broadcaster_->sendTransform(tf); -} + private: + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + std::unique_ptr broadcaster_; + geometry_msgs::msg::Transform transform_{geometry_msgs::msg::Transform()}; + std::string parent_frame_, child_frame_; + double offset_x_ = 0, offset_y_ = 0, offset_z_ = 0; +}; int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - rclcpp::TimerBase::SharedPtr timer = - rclcpp::create_timer(node, node->get_clock(), rclcpp::Duration(0, 1e7), [node]() -> void { node->step(); }); - rclcpp::experimental::executors::EventsExecutor exec; exec.add_node(node); exec.spin(); diff --git a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml index cdcdc4f72..15f4637e7 100644 --- a/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml +++ b/bitbots_misc/bitbots_ipm/config/soccer_ipm.yaml @@ -25,3 +25,4 @@ soccer_ipm: z: 1.0 output_frame: 'base_footprint' + use_distortion: true diff --git a/bitbots_misc/bitbots_ipm/launch/ipm.launch b/bitbots_misc/bitbots_ipm/launch/ipm.launch index 520c03a24..81b264066 100644 --- a/bitbots_misc/bitbots_ipm/launch/ipm.launch +++ b/bitbots_misc/bitbots_ipm/launch/ipm.launch @@ -1,17 +1,18 @@ - - - + + + - - + + + @@ -25,9 +26,10 @@ - - + + + diff --git a/bitbots_motion/bitbots_head_mover/src/move_head.cpp b/bitbots_motion/bitbots_head_mover/src/move_head.cpp index 8d9e61ef5..70e949b1c 100644 --- a/bitbots_motion/bitbots_head_mover/src/move_head.cpp +++ b/bitbots_motion/bitbots_head_mover/src/move_head.cpp @@ -102,7 +102,7 @@ class HeadMover { // Initialize subscriber for the current joint states of the robot joint_state_subscriber_ = node_->create_subscription( - "joint_states", 10, [this](const sensor_msgs::msg::JointState::SharedPtr msg) { joint_state_callback(msg); }); + "joint_states", 1, [this](const sensor_msgs::msg::JointState::SharedPtr msg) { joint_state_callback(msg); }); // Create parameter listener and load initial set of parameters param_listener_ = std::make_shared(node_); @@ -181,7 +181,7 @@ class HeadMover { std::bind(&HeadMover::handle_accepted, this, std::placeholders::_1)); // Initialize timer for main loop - timer_ = rclcpp::create_timer(node_, node_->get_clock(), 10ms, [this] { behave(); }); + timer_ = rclcpp::create_timer(node_, node_->get_clock(), 50ms, [this] { behave(); }); } /** @@ -689,7 +689,7 @@ class HeadMover { // Send the motor goals to the head motors bool success = - send_motor_goals(head_pan, head_tilt, true, pan_speed_, tilt_speed_, current_head_pan, current_head_tilt); + send_motor_goals(head_pan, head_tilt, false, pan_speed_, tilt_speed_, current_head_pan, current_head_tilt); if (success) { // Check if we reached the current keypoint and if so, increase the index diff --git a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml b/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml index f4709658e..11ae07cf1 100644 --- a/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml +++ b/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml @@ -21,9 +21,9 @@ bitbots_path_planning: max_rotation_vel: 0.4 max_vel_x: 0.1 min_vel_x: -0.05 - max_vel_y: 0.04 - smoothing_k: 0.03 + max_vel_y: 0.025 + smoothing_k: 0.05 rotation_i_factor: 0.0 - rotation_slow_down_factor: 0.4 + rotation_slow_down_factor: 2.0 translation_slow_down_factor: 0.5 orient_to_goal_distance: 1.0 diff --git a/bitbots_vision/bitbots_vision/vision.py b/bitbots_vision/bitbots_vision/vision.py index 5f476d84e..03891d5e7 100755 --- a/bitbots_vision/bitbots_vision/vision.py +++ b/bitbots_vision/bitbots_vision/vision.py @@ -125,7 +125,13 @@ def _set_up_vision_components(self, new_config: Dict) -> None: def _register_subscribers(self, config: Dict) -> None: self._sub_image = ros_utils.create_or_update_subscriber( - self, self._config, config, self._sub_image, "ROS_img_msg_topic", Image, callback=self._image_callback + self, + self._config, + config, + self._sub_image, + "ROS_img_msg_topic", + Image, + callback=self._image_callback, ) def _image_callback(self, image_msg: Image) -> None: diff --git a/bitbots_wolfgang/wolfgang_description/urdf/robot.urdf b/bitbots_wolfgang/wolfgang_description/urdf/robot.urdf index 31dc49076..b9fc59749 100644 --- a/bitbots_wolfgang/wolfgang_description/urdf/robot.urdf +++ b/bitbots_wolfgang/wolfgang_description/urdf/robot.urdf @@ -192,7 +192,7 @@ - + @@ -374,18 +374,19 @@ - + + - + - + @@ -3588,21 +3589,21 @@ + - + - + - --> @@ -3870,7 +3871,7 @@ - + true true @@ -3879,12 +3880,12 @@ __default_topic__ imu - imu_frame + imu_frame_uncalibrated 100.0 0.0 0 0 0 0 0 0 - imu_frame + imu_frame_uncalibrated 0 0 0 0 0 0 diff --git a/scripts/make_basler.sh b/scripts/make_basler.sh index acd6e9735..2bb77c08b 100755 --- a/scripts/make_basler.sh +++ b/scripts/make_basler.sh @@ -7,12 +7,8 @@ PYLON_DOWNLOAD_URL="https://www2.baslerweb.com/media/downloads/software/pylon_software/pylon_7_4_0_14900_linux_x86_64_debs.tar.gz" PYLON_VERSION="7.4.0" -# Similar to the pylon driver we also need to download the blaze supplementary package. -BLAZE_DOWNLOAD_URL="https://www2.baslerweb.com/media/downloads/software/tof_software/pylon-supplementary-package-for-blaze-1.5.0.def07388_amd64.deb" -BLAZE_VERSION="1.5.0" - # Check let the user confirm that they read the license agreement on the basler website and agree with it. -echo "You need to confirm that you read the license agreements for pylon $PYLON_VERSION and the blaze supplementary package $BLAZE_VERSION on the basler download page (https://www.baslerweb.com/en/downloads/software-downloads/) and agree with it." +echo "You need to confirm that you read the license agreements for pylon $PYLON_VERSION on the basler download page (https://www.baslerweb.com/en/downloads/software-downloads/) and agree with it." # Check --ci flag for automatic confirmation in the ci if [[ $1 == "--ci" ]]; then @@ -58,21 +54,3 @@ else # Install the pylon driver sudo apt-get install /tmp/pylon/pylon_${PYLON_VERSION}*.deb -y fi - -# Check if the correct blaze supplementary package BLAZE_VERSION is installed (apt) -if apt list pylon-supplementary-package-for-blaze --installed | grep -q $BLAZE_VERSION; then - echo "Blaze supplementary package $BLAZE_VERSION is already installed." -else - echo "Blaze supplementary package $BLAZE_VERSION is not installed. Installing..." - # Check if we have an internet connection - check_internet_connection $1 - # Check if the url exist - if ! curl --output /dev/null --silent --head --fail "$BLAZE_DOWNLOAD_URL"; then - echo "Blaze download url does not exist. Please check the url and update the 'BLAZE_DOWNLOAD_URL' variable in the 'make_basler.sh' script. The website might have changed." - exit 1 - fi - # Download the blaze supplementary package to temp folder - wget --no-verbose $SHOW_PROGRESS $BLAZE_DOWNLOAD_URL -O /tmp/pylon-blaze-supplementary-package_${BLAZE_VERSION}.deb - # Install the blaze supplementary package - sudo apt-get install /tmp/pylon-blaze-supplementary-package_${BLAZE_VERSION}*.deb -y -fi diff --git a/sync_includes_wolfgang_nuc.yaml b/sync_includes_wolfgang_nuc.yaml index a212b8f5a..d08677309 100644 --- a/sync_includes_wolfgang_nuc.yaml +++ b/sync_includes_wolfgang_nuc.yaml @@ -55,7 +55,6 @@ include: - imu_tools - ipm - particle_filter - - pylon-ros-camera - ros2_numpy - ros2_python_extension - soccer_ipm diff --git a/workspace.repos b/workspace.repos index 3d85c349b..7f19dfe07 100644 --- a/workspace.repos +++ b/workspace.repos @@ -58,10 +58,6 @@ repositories: type: git url: git@github.com:ros-sports/ipm.git version: rolling - lib/pylon-ros-camera: - type: git - url: git@github.com:basler/pylon-ros-camera.git - version: humble lib/ros2_numpy: type: git url: git@github.com:Bit-Bots/ros2_numpy.git