Skip to content

Commit

Permalink
ToF Support Humble (#581)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Serafadam
Serafadam authored Aug 23, 2024
1 parent b897bff commit 1d526be
Show file tree
Hide file tree
Showing 20 changed files with 646 additions and 88 deletions.
166 changes: 92 additions & 74 deletions depthai_descriptions/urdf/include/base_macro.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,92 +1,110 @@
<?xml version="1.0"?>
<robot name="base"
xmlns:xacro="http://ros.org/wiki/xacro">
xmlns:xacro="http://ros.org/wiki/xacro">

<xacro:macro name="base" params="camera_name camera_model parent base_frame
<xacro:macro name="base"
params="camera_name camera_model parent base_frame
cam_pos_x cam_pos_y cam_pos_z
cam_roll cam_pitch cam_yaw has_imu r:=0.8 g:=0.8 b:=0.8 a:=0.8 simulation:=false">

<xacro:if value="${simulation}">
<xacro:property
name="file_prefix"
value="file://$(find depthai_descriptions)"
/>
</xacro:if>
<xacro:unless value="${simulation}">
<xacro:property
name="file_prefix"
value="package://depthai_descriptions"
/>
</xacro:unless>
<xacro:if value="${simulation}">
<xacro:property
name="file_prefix"
value="file://$(find depthai_descriptions)"
/>
</xacro:if>
<xacro:unless value="${simulation}">
<xacro:property
name="file_prefix"
value="package://depthai_descriptions"
/>
</xacro:unless>

<!-- base_link of the sensor-->
<link name="${base_frame}"/>
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:property name="model" value="${camera_model}" />
<!-- base_link of the sensor-->
<link name="${base_frame}" />
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:property name="model" value="${camera_model}" />

<joint name="${camera_name}_center_joint" type="fixed">
<parent link="${parent}"/>
<child link="${base_frame}"/>
<origin xyz="${cam_pos_x} ${cam_pos_y} ${cam_pos_z}" rpy="${cam_roll} ${cam_pitch} ${cam_yaw}" />
</joint>
<joint name="${camera_name}_center_joint" type="fixed">
<parent link="${parent}" />
<child link="${base_frame}" />
<origin xyz="${cam_pos_x} ${cam_pos_y} ${cam_pos_z}"
rpy="${cam_roll} ${cam_pitch} ${cam_yaw}" />
</joint>

<!-- device Center -->
<link name="${camera_name}_model_origin">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="${file_prefix}/urdf/models/${model}.stl" />
</geometry>
<material name="mat">
<color rgba="${r} ${g} ${b} ${a}"/>
</material>
</visual>
</link>
<!-- device Center -->
<xacro:unless value="${model in ['OAK-D-SR-POE']}">
<link name="${camera_name}_model_origin">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${file_prefix}/urdf/models/${model}.stl" />
</geometry>
<material name="mat">
<color rgba="${r} ${g} ${b} ${a}" />
</material>
</visual>
</link>
</xacro:unless>
<xacro:if value="${model in ['OAK-D-SR-POE']}">
<link name="${camera_name}_model_origin">
<visual>
<origin xyz="0.019 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="${file_prefix}/urdf/models/${model}.stl" />
</geometry>
<material name="mat">
<color rgba="${r} ${g} ${b} ${a}" />
</material>
</visual>
</link>
</xacro:if>

<joint name="${camera_name}_model_origin_joint" type="fixed">
<parent link="${base_frame}"/>
<child link="${camera_name}_model_origin"/>
<origin xyz="0 0 0" rpy="1.5708 0 1.5708" />
</joint>
<joint name="${camera_name}_model_origin_joint" type="fixed">
<parent link="${base_frame}" />
<child link="${camera_name}_model_origin" />
<origin xyz="0 0 0" rpy="1.5708 0 1.5708" />
</joint>


<!-- IMU -->
<!-- IMU -->

<xacro:if value="${model == 'OAK-D'}">
<xacro:property name="imu_offset_x" value="0.0" />
<xacro:property name="imu_offset_y" value="-0.015" />
<xacro:property name="imu_offset_z" value="-0.013662" />
<xacro:property name="imu_r" value="0.0" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>
<xacro:if value="${model == 'OAK-D'}">
<xacro:property name="imu_offset_x" value="0.0" />
<xacro:property name="imu_offset_y" value="-0.015" />
<xacro:property name="imu_offset_z" value="-0.013662" />
<xacro:property name="imu_r" value="0.0" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>

<xacro:if value="${model == 'OAK-D-PRO'}">
<xacro:property name="imu_offset_x" value="-0.008" />
<xacro:property name="imu_offset_y" value="-0.037945" />
<xacro:property name="imu_offset_z" value="-0.00079" />
<xacro:property name="imu_r" value="${M_PI}" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>
<xacro:if value="${model == 'OAK-D-PRO'}">
<xacro:property name="imu_offset_x" value="-0.008" />
<xacro:property name="imu_offset_y" value="-0.037945" />
<xacro:property name="imu_offset_z" value="-0.00079" />
<xacro:property name="imu_r" value="${M_PI}" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>

<xacro:if value="${model == 'OAK-D-POE'}">
<xacro:property name="imu_offset_x" value="-0.008" />
<xacro:property name="imu_offset_y" value="-0.04" />
<xacro:property name="imu_offset_z" value="-0.020265" />
<xacro:property name="imu_r" value="${M_PI}" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>
<xacro:if value="${model == 'OAK-D-POE'}">
<xacro:property name="imu_offset_x" value="-0.008" />
<xacro:property name="imu_offset_y" value="-0.04" />
<xacro:property name="imu_offset_z" value="-0.020265" />
<xacro:property name="imu_r" value="${M_PI}" />
<xacro:property name="imu_p" value="${M_PI/2.0}" />
<xacro:property name="imu_y" value="0.0" />
</xacro:if>

<xacro:if value="${has_imu}">
<link name="${camera_name}_imu_frame" />
<joint name="${camera_name}_imu_joint" type="fixed">
<parent link="${base_frame}"/>
<child link="${camera_name}_imu_frame"/>
<origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}" rpy="${imu_r} ${imu_p} ${imu_y}" />
</joint>
</xacro:if>
<xacro:if value="${has_imu}">
<link name="${camera_name}_imu_frame" />
<joint name="${camera_name}_imu_joint" type="fixed">
<parent link="${base_frame}" />
<child link="${camera_name}_imu_frame" />
<origin xyz="${imu_offset_x} ${imu_offset_y} ${imu_offset_z}"
rpy="${imu_r} ${imu_p} ${imu_y}" />
</joint>
</xacro:if>

</xacro:macro>
</xacro:macro>
</robot>
40 changes: 39 additions & 1 deletion depthai_descriptions/urdf/include/depthai_macro.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,8 @@
<child link="${camera_name}_${rgb_name}_${optical_frame_suffix}" />
</joint>
</xacro:unless>
<xacro:unless value="${model in ['OAK-D-LR']}">
<!-- LR and SR POE have different baselines and positions -->
<xacro:unless value="${model in ['OAK-D-LR', 'OAK-D-SR-POE']}">

<!-- Left Camera -->
<link name="${camera_name}_${left_name}_${frame_suffix}" />
Expand Down Expand Up @@ -134,6 +135,43 @@
<child link="${camera_name}_${right_name}_${optical_frame_suffix}" />
</joint>
</xacro:if>

<xacro:if value="${model in ['OAK-D-SR-POE']}">

<!-- left Camera -->
<link name="${camera_name}_${left_name}_${frame_suffix}" />

<joint name="${camera_name}_left_camera_joint" type="fixed">
<parent link="${base_frame}" />
<child link="${camera_name}_${left_name}_${frame_suffix}" />
<origin xyz="0 0.0375 0" rpy="0 0 0" />
</joint>

<link name="${camera_name}_${left_name}_${optical_frame_suffix}" />

<joint name="${camera_name}_left_camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}" />
<parent link="${camera_name}_${left_name}_${frame_suffix}" />
<child link="${camera_name}_${left_name}_${optical_frame_suffix}" />
</joint>

<!-- right Camera -->
<link name="${camera_name}_${right_name}_${frame_suffix}" />

<joint name="${camera_name}_right_camera_joint" type="fixed">
<parent link="${base_frame}" />
<child link="${camera_name}_${right_name}_${frame_suffix}" />
<origin xyz="0 0.0175 0" rpy="0 0 0" />
</joint>

<link name="${camera_name}_${right_name}_${optical_frame_suffix}" />

<joint name="${camera_name}_right_camera_optical_joint" type="fixed">
<origin xyz="0 0 0" rpy="-${M_PI/2} 0.0 -${M_PI/2}" />
<parent link="${camera_name}_${right_name}_${frame_suffix}" />
<child link="${camera_name}_${right_name}_${optical_frame_suffix}" />
</joint>
</xacro:if>
</xacro:macro>

</robot>
Binary file added depthai_descriptions/urdf/models/OAK-D-SR-POE.stl
View file
Binary file not shown.
4 changes: 3 additions & 1 deletion depthai_ros_driver/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ add_library(
src/param_handlers/sensor_param_handler.cpp
src/param_handlers/feature_tracker_param_handler.cpp
src/param_handlers/stereo_param_handler.cpp
src/param_handlers/tof_param_handler.cpp
src/param_handlers/pipeline_gen_param_handler.cpp
src/param_handlers/sync_param_handler.cpp
)
Expand All @@ -89,8 +90,9 @@ add_library(
src/dai_nodes/sensors/mono.cpp
src/dai_nodes/sensors/feature_tracker.cpp
src/dai_nodes/sensors/sensor_wrapper.cpp
src/dai_nodes/sensors/stereo.cpp
src/dai_nodes/sensors/tof.cpp
src/dai_nodes/sensors/sync.cpp
src/dai_nodes/stereo.cpp
)

ament_target_dependencies(${SENSOR_LIB_NAME} ${SENSOR_DEPS})
Expand Down
10 changes: 10 additions & 0 deletions depthai_ros_driver/config/sr_poe_rgbd.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@

/oak:
ros__parameters:
camera:
i_enable_imu: true
i_enable_ir: false
i_nn_type: none
i_pipeline_type: rgbtof
right:
i_low_bandwidth: true
0 ...e/depthai_ros_driver/dai_nodes/stereo.hpp → ...i_ros_driver/dai_nodes/sensors/stereo.hpp
View file
File renamed without changes.
60 changes: 60 additions & 0 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/tof.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
#pragma once

#include "depthai_ros_driver/dai_nodes/base_node.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"

namespace dai {
class Pipeline;
class Device;
class DataInputQueue;
class ADatatype;
namespace node {
class Camera;
class ToF;
class ImageAlign;
class XLinkIn;
} // namespace node
} // namespace dai

namespace rclcpp {
class Node;
class Parameter;
} // namespace rclcpp

namespace depthai_ros_driver {
namespace param_handlers {
class ToFParamHandler;
}
namespace dai_nodes {

namespace sensor_helpers {
class ImagePublisher;
} // namespace sensor_helpers
class ToF : public BaseNode {
public:
explicit ToF(const std::string& daiNodeName,
std::shared_ptr<rclcpp::Node> node,
std::shared_ptr<dai::Pipeline> pipeline,
dai::CameraBoardSocket boardSocket = dai::CameraBoardSocket::CAM_A);
~ToF();
void updateParams(const std::vector<rclcpp::Parameter>& params) override;
void setupQueues(std::shared_ptr<dai::Device> device) override;
void link(dai::Node::Input in, int linkType = 0) override;
dai::Node::Input getInput(int linkType = 0) override;
void setNames() override;
void setXinXout(std::shared_ptr<dai::Pipeline> pipeline) override;
std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> getPublishers() override;
void closeQueues() override;

private:
std::shared_ptr<sensor_helpers::ImagePublisher> tofPub;
std::shared_ptr<dai::node::Camera> camNode;
std::shared_ptr<dai::node::ToF> tofNode;
std::shared_ptr<dai::node::ImageAlign> alignNode;
std::unique_ptr<param_handlers::ToFParamHandler> ph;
dai::CameraBoardSocket boardSocket;
std::string tofQName;
};

} // namespace dai_nodes
} // namespace depthai_ros_driver
35 changes: 35 additions & 0 deletions depthai_ros_driver/include/depthai_ros_driver/param_handlers/tof_param_handler.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
#pragma once

#include <memory>
#include <string>
#include <vector>

#include "depthai/pipeline/datatype/CameraControl.hpp"
#include "depthai/pipeline/datatype/ToFConfig.hpp"
#include "depthai_ros_driver/param_handlers/base_param_handler.hpp"

namespace dai {
namespace node {
class Camera;
class ToF;
} // namespace node
} // namespace dai

namespace rclcpp {
class Node;
class Parameter;
} // namespace rclcpp

namespace depthai_ros_driver {
namespace param_handlers {

class ToFParamHandler : public BaseParamHandler {
public:
explicit ToFParamHandler(std::shared_ptr<rclcpp::Node> node, const std::string& name);
~ToFParamHandler();
void declareParams(std::shared_ptr<dai::node::Camera> cam, std::shared_ptr<dai::node::ToF> tof);
dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;
std::unordered_map<std::string, dai::MedianFilter> medianFilterMap;
};
} // namespace param_handlers
} // namespace depthai_ros_driver
2 changes: 1 addition & 1 deletion depthai_ros_driver/include/depthai_ros_driver/pipeline/base_pipeline.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
#include "depthai_ros_driver/dai_nodes/nn/nn_wrapper.hpp"
#include "depthai_ros_driver/dai_nodes/nn/spatial_nn_wrapper.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp"
#include "depthai_ros_driver/dai_nodes/stereo.hpp"
#include "depthai_ros_driver/dai_nodes/sensors/stereo.hpp"

namespace dai {
class Pipeline;
Expand Down
Loading

0 comments on commit 1d526be

Please sign in to comment.