diff --git a/src/hollander_bringup/config/perception.config.yaml b/src/hollander_bringup/config/perception.config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..186db237f95cc862dfebd2c738624d0e9b034b3d
--- /dev/null
+++ b/src/hollander_bringup/config/perception.config.yaml
@@ -0,0 +1,113 @@
+/**:
+ ros__parameters:
+ # SETTINGS
+ base_frame_id: "link" # Define the frame_id all static transformations refers to.
+ odom_frame_id: "odom" # Define the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). Pose topic defines the pose relative to that system.
+ initial_reset: true # Allows resetting the device before using it. Recommended to be used for example, when a glitch prevented the device from being properly closed.
+ wait_for_device_timeout: -1.0 # Timeout for waiting for device to connect [seconds]
+ reconnect_timeout: 5.0 # Timeout between consecutive reconnection attempts [seconds]
+ enable_sync: false # ??
+
+hollander:
+ cameras:
+ front:
+ camera_1571: # LINK https://dev.intelrealsense.com/docs/ros-wrapper#section-launch-parameters
+ ros__parameters:
+ # CAMERA IDENTITY
+ serial_no: "008222071571" # Attach to the device with the given serial number (serial_no) number.
+ camera_name: "camera_1571"
+
+ # TF
+ publish_tf: true # A Boolean parameter, publish or not TF at all.
+ tf_prefix: "front_camera" # This parameter allows changing the frames' IDs prefix per camera.
+ tf_publish_rate: 0.0 # A double parameter, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication.
+ publish_odom_tf: true # A Boolean parameter, publish if True TF from odom_frame to pose_frame.
+
+ # IMU
+ unite_imu_method: 2 # [0-None, 1-copy, 2-linear_interpolation]
+ linear_accel_cov: 0.01 # Set the variance given to the Imu readings.
+ angular_velocity_cov: 0.01
+
+ enable_gyro: true # Enable gyro stream
+ gyro_fps: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ enable_accel: true # Enable accel stream
+ accel_fps: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+
+ # DEPTH
+ enable_depth: true # Enable depth stream
+ depth_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ depth_height: 0
+ depth_fps: 0
+ clip_distance: 5.0 # Remove from the depth image all values above a given value (meters).
+ align_depth: true # If set to true, will publish additional topics with all the images aligned to the depth image.
+
+ pointcloud.enable: true
+ depth_module.visual_preset: 3
+
+ #COLOR
+ enable_color: true # Enable color stream
+ color_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ color_height: 0
+ color_fps: 0
+
+ # INFRA
+ enable_infra1: false # Enable infra1 stream
+ enable_infra2: false # Enable infra2 stream
+ infra_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ infra_height: 0
+ infra_fps: 0
+
+ back:
+ camera_1502: # LINK https://dev.intelrealsense.com/docs/ros-wrapper#section-launch-parameters
+ ros__parameters:
+ # CAMERA IDENTITY
+ serial_no: "008222071502" # Attach to the device with the given serial number (serial_no) number.
+ camera_name: "camera_1502"
+
+ # SETTINGS
+ base_frame_id: "link" # Define the frame_id all static transformations refers to.
+ odom_frame_id: "odom" # Define the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). Pose topic defines the pose relative to that system.
+ initial_reset: false # Allows resetting the device before using it. Recommended to be used for example, when a glitch prevented the device from being properly closed.
+ wait_for_device_timeout: -1.0 # Timeout for waiting for device to connect [seconds]
+ reconnect_timeout: 5.0 # Timeout between consecutive reconnection attempts [seconds]
+ enable_sync: false # ??
+
+ # TF
+ publish_tf: true # A Boolean parameter, publish or not TF at all.
+ tf_prefix: "back_camera" # This parameter allows changing the frames' IDs prefix per camera.
+ tf_publish_rate: 0.0 # A double parameter, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication.
+ publish_odom_tf: true # A Boolean parameter, publish if True TF from odom_frame to pose_frame.
+
+ # IMU
+ unite_imu_method: 2 # [0-None, 1-copy, 2-linear_interpolation]
+ linear_accel_cov: 0.01 # Set the variance given to the Imu readings.
+ angular_velocity_cov: 0.01
+
+ enable_gyro: true # Enable gyro stream
+ gyro_fps: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ enable_accel: true # Enable accel stream
+ accel_fps: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+
+ # DEPTH
+ enable_depth: true # Enable depth stream
+ depth_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ depth_height: 0
+ depth_fps: 0
+ clip_distance: 5.0 # Remove from the depth image all values above a given value (meters).
+ align_depth: true # If set to true, will publish additional topics with all the images aligned to the depth image.
+
+ pointcloud.enable: true
+ depth_module.visual_preset: 3
+
+ #COLOR
+ enable_color: true # Enable color stream
+ color_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ color_height: 0
+ color_fps: 0
+
+ # INFRA
+ enable_infra1: false # Enable infra1 stream
+ enable_infra2: false # Enable infra2 stream
+ infra_width: 0 # Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
+ infra_height: 0
+ infra_fps: 0
\ No newline at end of file
diff --git a/src/hollander_bringup/config/teleop.joy.config.yaml b/src/hollander_bringup/config/teleop.joy.config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1a2c8a6b0c78b8ba8cce771683a294faa48134e2
--- /dev/null
+++ b/src/hollander_bringup/config/teleop.joy.config.yaml
@@ -0,0 +1,29 @@
+hollander:
+ joy_node:
+ ros__parameters:
+ dev: "/dev/input/js0"
+ deadzone: 0.15
+ autorepeat_rate: 20.0
+
+ teleop_hollander_joy:
+ ros__parameters:
+ buttons_publish_frequency: 10.0
+
+ teleop_twist_joy:
+ ros__parameters:
+ axis_linear: # Left thumb stick vertical
+ x: 1
+ scale_linear:
+ x: 0.3
+ scale_linear_turbo:
+ x: 0.7
+
+ axis_angular: # Left thumb stick horizontal
+ yaw: 0
+ scale_angular:
+ yaw: 0.2
+ scale_angular_turbo:
+ yaw: 0.4
+
+ enable_button: 5 # Left trigger button
+ enable_turbo_button: 4 # Right trigger button
diff --git a/src/hollander_bringup/launch/perception.launch.py b/src/hollander_bringup/launch/perception.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..155fb368cc1013ea756671d61d5ac459842627fa
--- /dev/null
+++ b/src/hollander_bringup/launch/perception.launch.py
@@ -0,0 +1,62 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch import LaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+ # Get launch configuration
+ launch_dir = get_package_share_directory('hollander_bringup')
+ default_config_file = os.path.join(launch_dir, 'config', 'perception.config.yaml')
+
+ # Parameters
+ configurable_parameters = [
+ {'name': 'front_camera_name', 'default': 'camera_1571', 'description': 'camera unique name'},
+ {'name': 'back_camera_name', 'default': 'camera_1502', 'description': 'camera unique name'},
+ {'name': 'front_namespace', 'default': 'hollander/cameras/front', 'description': 'Namespace to avoide conflicts'},
+ {'name': 'back_namespace', 'default': 'hollander/cameras/back', 'description': 'Namespace to avoide conflicts'},
+ {'name': 'config', 'default': default_config_file, 'description': 'Namespace to avoide conflicts'},
+ {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
+ ]
+
+ def declare_configurable_parameters(parameters):
+ return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
+
+ front_camera = Node(
+ package='realsense2_camera',
+ executable='realsense2_camera_node',
+ name=LaunchConfiguration("front_camera_name"),
+ namespace=LaunchConfiguration("front_namespace"),
+ parameters=[LaunchConfiguration("config")],
+ output='screen',
+ arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
+ emulate_tty=True,
+ )
+
+ back_camera = Node(
+ package='realsense2_camera',
+ executable='realsense2_camera_node',
+ name=LaunchConfiguration("back_camera_name"),
+ namespace=LaunchConfiguration("back_namespace"),
+ parameters=[LaunchConfiguration("config")],
+ output='screen',
+ arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')],
+ emulate_tty=True,
+ )
+
+ pointclouds_to_laserscan = IncludeLaunchDescription(
+ PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('hollander_bringup'), 'launch', 'ptl.launch.py')),
+ condition=IfCondition(LaunchConfiguration('need_description'))
+ )
+
+ return LaunchDescription( declare_configurable_parameters(configurable_parameters) + [
+ front_camera,
+ back_camera,
+
+ pointclouds_to_laserscan,
+ ])
diff --git a/src/hollander_bringup/launch/teleop.joy.launch.py b/src/hollander_bringup/launch/teleop.joy.launch.py
new file mode 100644
index 0000000000000000000000000000000000000000..5d86e623027b8c3bc9e179a6cd1a744dc2abf2db
--- /dev/null
+++ b/src/hollander_bringup/launch/teleop.joy.launch.py
@@ -0,0 +1,54 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+
+ # Get launch configuration
+ launch_dir = get_package_share_directory('hollander_bringup')
+ default_config_file = os.path.join(launch_dir, 'config', 'teleop.joy.config.yaml')
+
+ # Parametersros
+ configurable_parameters = [
+ {'name': 'namespace', 'default': 'hollander', 'description': 'Namespace to avoide conflicts'},
+ {'name': 'config', 'default': default_config_file, 'description': 'Namespace to avoide conflicts'},
+ {'name': 'log_level', 'default': 'info', 'description': 'debug log level [DEBUG|INFO|WARN|ERROR|FATAL]'},
+ ]
+
+ def declare_configurable_parameters(parameters):
+ return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters]
+
+
+ joy_node = Node(
+ package='joy',
+ executable='joy_node',
+ name='joy_node',
+ namespace=LaunchConfiguration("namespace"),
+ parameters=[default_config_file],
+ )
+
+ hollander_teleop_joy_node = Node(
+ package='hollander_teleop',
+ executable='hollander_teleop_joy',
+ name='teleop_hollander_joy',
+ namespace=LaunchConfiguration("namespace"),
+ parameters=[default_config_file],
+ )
+
+ teleop_twist_joy_node = Node(
+ package='teleop_twist_joy',
+ executable='teleop_node',
+ name='teleop_twist_joy',
+ namespace=LaunchConfiguration("namespace"),
+ parameters=[default_config_file],
+ )
+
+ return LaunchDescription( declare_configurable_parameters(configurable_parameters) + [
+ joy_node,
+ hollander_teleop_joy_node,
+ teleop_twist_joy_node,
+ ])
diff --git a/src/puntenwolk_fusie/include/puntenwolk_fusie/puntenwolk_fusie.hpp b/src/puntenwolk_fusie/include/puntenwolk_fusie/puntenwolk_fusie.hpp
index 529daba51855cfdeed591501f846b9dd2fbdd50d..6e66e2bbf1c9f2f79980d5e726cd157255d30aea 100644
--- a/src/puntenwolk_fusie/include/puntenwolk_fusie/puntenwolk_fusie.hpp
+++ b/src/puntenwolk_fusie/include/puntenwolk_fusie/puntenwolk_fusie.hpp
@@ -65,7 +65,7 @@ class PuntenwolkFusie : public lc::LifecycleNode {
lc::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_publisher;
// TF Listener
- std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+ std::shared_ptr<const tf2_ros::TransformListener> transform_listener_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
// Subscribe
diff --git a/src/puntenwolk_fusie/package.xml b/src/puntenwolk_fusie/package.xml
index 29b53b7877e399f9854fa5cd003ccd3c7d8f9ff7..b34622328eaeebe628f4adbe916467237fb4a15e 100644
--- a/src/puntenwolk_fusie/package.xml
+++ b/src/puntenwolk_fusie/package.xml
@@ -3,9 +3,9 @@
<package format="3">
<name>puntenwolk_fusie</name>
<version>0.0.0</version>
- <description>TODO: Package description</description>
- <maintainer email="galambos.mihaly@hallgato.ppke.hu">galmi</maintainer>
- <license>TODO: License declaration</license>
+ <description>Node for merging two sensor_msgs/PointCloud2 message.</description>
+ <maintainer email="galambos.mihaly@hallgato.ppke.hu">Mihaly Galambos</maintainer>
+ <license>Apache 2.0</license>
<buildtool_depend>ament_cmake_auto</buildtool_depend>
@@ -13,6 +13,7 @@
<depend>rclcpp_components</depend>
<depend>rclcpp_lifecycle</depend>
<depend>lifecycle_msgs</depend>
+ <depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>sensor_msgs</depend>
<depend>pcl_conversions</depend>
diff --git a/src/puntenwolk_fusie/src/puntenwolk_fusie.cpp b/src/puntenwolk_fusie/src/puntenwolk_fusie.cpp
index 8e0369144a9836212d602c56d9ac0112c0789de0..e8e03ca2ea11a70a4823a207d1b852f32418fc5a 100644
--- a/src/puntenwolk_fusie/src/puntenwolk_fusie.cpp
+++ b/src/puntenwolk_fusie/src/puntenwolk_fusie.cpp
@@ -2,11 +2,12 @@
// INCLUDE - Common
#include "pcl_conversions/pcl_conversions.h"
#include "pcl_ros/transforms.hpp"
+#include "tf2/exceptions.h"
// INCLUDE - Header
#include "puntenwolk_fusie/puntenwolk_fusie.hpp"
-PuntenwolkFusie::PuntenwolkFusie(rclcpp::NodeOptions options) : lc::LifecycleNode("odometry_publisher", options) {
+PuntenwolkFusie::PuntenwolkFusie(rclcpp::NodeOptions options) : lc::LifecycleNode("puntenwolk_fusie", options) {
/// Parameters
this->declare_all_parameters();
@@ -18,7 +19,7 @@ LNI::CallbackReturn PuntenwolkFusie::on_configure(const lc::State& state) {
(void)state;
// Publisher
- cloud_publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(cloud_out_topic_name, 10);
+ cloud_publisher = this->create_publisher<sensor_msgs::msg::PointCloud2>(cloud_out_topic_name, rclcpp::SensorDataQoS());
// Timers
cloud_publisher_timer = this->create_wall_timer(std::chrono::milliseconds((int)(1000.0 / publish_frequency)), std::bind(&PuntenwolkFusie::merge_clouds, this));
@@ -84,10 +85,12 @@ void PuntenwolkFusie::declare_all_parameters() {
void PuntenwolkFusie::cloud_in_1_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
cloud_in_1 = msg;
+ RCLCPP_DEBUG(this->get_logger(), "Cloud in 1 was received!");
}
void PuntenwolkFusie::cloud_in_2_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
cloud_in_2 = msg;
+ RCLCPP_DEBUG(this->get_logger(), "Cloud in 2 was received!");
}
void PuntenwolkFusie::merge_clouds() {
@@ -95,16 +98,24 @@ void PuntenwolkFusie::merge_clouds() {
sensor_msgs::msg::PointCloud2 transformed_cloud_2;
if (cloud_in_1 != NULL && cloud_in_2 != NULL) {
+ RCLCPP_DEBUG(this->get_logger(), "Both cloud are OK");
+
bool success = false;
- success &= pcl_ros::transformPointCloud(target_frame_id, *cloud_in_1, transformed_cloud_1, *tf_buffer_);
- success &= pcl_ros::transformPointCloud(target_frame_id, *cloud_in_2, transformed_cloud_2, *tf_buffer_);
+ success = pcl_ros::transformPointCloud(target_frame_id, *cloud_in_1, transformed_cloud_1, *tf_buffer_);
+ RCLCPP_DEBUG(this->get_logger(), "First transform: %d", success);
+
+ success = pcl_ros::transformPointCloud(target_frame_id, *cloud_in_2, transformed_cloud_2, *tf_buffer_);
+ RCLCPP_DEBUG(this->get_logger(), "Second transform: %d", success);
sensor_msgs::msg::PointCloud2 msg;
- success &= pcl::concatenatePointCloud(transformed_cloud_1, transformed_cloud_2, msg);
+ success = pcl::concatenatePointCloud(transformed_cloud_1, transformed_cloud_2, msg);
+ RCLCPP_DEBUG(this->get_logger(), "Merge: %d", success);
if (success && cloud_publisher->is_activated()) {
+ RCLCPP_DEBUG(this->get_logger(), "Publishing merged cloud");
+
cloud_publisher->publish(msg);
}
}