Skip to content
This repository has been archived by the owner on Jan 7, 2023. It is now read-only.

Commit

Permalink
Merge pull request #50 from ahuizxc/dashing
Browse files Browse the repository at this point in the history 
fix compiling failure after ros2 core updated and remove some warnings.
  • Loading branch information
@xhuan28
xhuan28 authored May 30, 2019
2 parents 384a645 + ccd00fe commit 9cc4723
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 19 deletions.
10 changes: 5 additions & 5 deletions realsense_ros2_camera/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,8 @@ if(UNIX OR APPLE)
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -pie")
endif()

find_package(realsense2)
if(NOT realsense2_FOUND)
find_package(librealsense2)
if(NOT librealsense2_FOUND)
message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n")
endif()

Expand Down Expand Up @@ -86,7 +86,7 @@ include_directories(
${cv_bridge_INCLUDE_DIRS}
${tf2_ros_INCLUDE_DIRS}
${tf2_INCLUDE_DIRS}
${realsense2_INCLUDE_DIRS}
${librealsense2_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}
Expand All @@ -103,7 +103,7 @@ target_link_libraries(${PROJECT_NAME}
${tf2_LIBRARIES}
${realsense_camera_msgs_LIBRARIES}
${cv_bridge_LIBRARIES}
realsense2
${librealsense2_LIBRARIES}
)
ament_target_dependencies(${PROJECT_NAME}
rclcpp
Expand All @@ -116,7 +116,7 @@ ament_target_dependencies(${PROJECT_NAME}
cv_bridge
OpenCV
image_transport
realsense2
librealsense2
)

# Install binaries
Expand Down
33 changes: 19 additions & 14 deletions realsense_ros2_camera/src/realsense_camera_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,8 @@ class RealSenseCameraNode : public rclcpp::Node
: Node("RealSenseCameraNode"),
_ros_clock(RCL_ROS_TIME),
_serial_no(""),
_static_tf_broadcaster(std::shared_ptr<rclcpp::Node>(this)),
_base_frame_id(""),
qos(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)),
_intialize_time_base(false)
{
RCLCPP_INFO(logger_, "RealSense ROS v%s", REALSENSE_ROS_VERSION_STR);
Expand Down Expand Up @@ -419,30 +419,32 @@ class RealSenseCameraNode : public rclcpp::Node
_info_publisher[FISHEYE] = this->create_publisher<sensor_msgs::msg::CameraInfo>(
"camera/fisheye/camera_info", 1);
_fe_to_depth_publisher = this->create_publisher<realsense_camera_msgs::msg::Extrinsics>(
"camera/extrinsics/fisheye2depth", rmw_qos_profile_default);
"camera/extrinsics/fisheye2depth", qos);
}

if (true == _enable[GYRO]) {
_imu_publishers[GYRO] = this->create_publisher<sensor_msgs::msg::Imu>("camera/gyro/sample",
100);
_imu_info_publisher[GYRO] = this->create_publisher<realsense_camera_msgs::msg::IMUInfo>(
"camera/gyro/imu_info", rmw_qos_profile_default);
"camera/gyro/imu_info", qos);
}

if (true == _enable[ACCEL]) {
_imu_publishers[ACCEL] = this->create_publisher<sensor_msgs::msg::Imu>("camera/accel/sample",
100);
_imu_info_publisher[ACCEL] = this->create_publisher<realsense_camera_msgs::msg::IMUInfo>(
"camera/accel/imu_info", rmw_qos_profile_default);
"camera/accel/imu_info", qos);
}

if (true == _enable[FISHEYE] &&
(true == _enable[GYRO] ||
true == _enable[ACCEL]))
{
_fe_to_imu_publisher = this->create_publisher<realsense_camera_msgs::msg::Extrinsics>(
"camera/extrinsics/fisheye2imu", rmw_qos_profile_default);
"camera/extrinsics/fisheye2imu", qos);
}
_static_tf_broadcaster_ =
std::make_shared<tf2_ros::StaticTransformBroadcaster>(shared_from_this());
}

void setupStreams()
Expand Down Expand Up @@ -812,7 +814,7 @@ class RealSenseCameraNode : public rclcpp::Node
b2d_msg.transform.rotation.y = 0;
b2d_msg.transform.rotation.z = 0;
b2d_msg.transform.rotation.w = 1;
_static_tf_broadcaster.sendTransform(b2d_msg);
_static_tf_broadcaster_->sendTransform(b2d_msg);

// Transform depth frame to depth optical frame
q_d2do.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
Expand All @@ -826,7 +828,7 @@ class RealSenseCameraNode : public rclcpp::Node
d2do_msg.transform.rotation.y = q_d2do.getY();
d2do_msg.transform.rotation.z = q_d2do.getZ();
d2do_msg.transform.rotation.w = q_d2do.getW();
_static_tf_broadcaster.sendTransform(d2do_msg);
_static_tf_broadcaster_->sendTransform(d2do_msg);


if (true == _enable[COLOR]) {
Expand All @@ -843,7 +845,7 @@ class RealSenseCameraNode : public rclcpp::Node
b2c_msg.transform.rotation.y = q.y();
b2c_msg.transform.rotation.z = q.z();
b2c_msg.transform.rotation.w = q.w();
_static_tf_broadcaster.sendTransform(b2c_msg);
_static_tf_broadcaster_->sendTransform(b2c_msg);

// Transform color frame to color optical frame
q_c2co.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
Expand All @@ -857,7 +859,7 @@ class RealSenseCameraNode : public rclcpp::Node
c2co_msg.transform.rotation.y = q_c2co.getY();
c2co_msg.transform.rotation.z = q_c2co.getZ();
c2co_msg.transform.rotation.w = q_c2co.getW();
_static_tf_broadcaster.sendTransform(c2co_msg);
_static_tf_broadcaster_->sendTransform(c2co_msg);
}

if (_enable[DEPTH]) {
Expand All @@ -883,7 +885,7 @@ class RealSenseCameraNode : public rclcpp::Node
b2ir1_msg.transform.rotation.y = q.y();
b2ir1_msg.transform.rotation.z = q.z();
b2ir1_msg.transform.rotation.w = q.w();
_static_tf_broadcaster.sendTransform(b2ir1_msg);
_static_tf_broadcaster_->sendTransform(b2ir1_msg);

// Transform infra1 frame to infra1 optical frame
ir1_2_ir1o.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
Expand All @@ -897,7 +899,7 @@ class RealSenseCameraNode : public rclcpp::Node
ir1_2_ir1o_msg.transform.rotation.y = ir1_2_ir1o.getY();
ir1_2_ir1o_msg.transform.rotation.z = ir1_2_ir1o.getZ();
ir1_2_ir1o_msg.transform.rotation.w = ir1_2_ir1o.getW();
_static_tf_broadcaster.sendTransform(ir1_2_ir1o_msg);
_static_tf_broadcaster_->sendTransform(ir1_2_ir1o_msg);
}

if (true == _enable[INFRA2]) {
Expand All @@ -915,7 +917,7 @@ class RealSenseCameraNode : public rclcpp::Node
b2ir2_msg.transform.rotation.y = q.y();
b2ir2_msg.transform.rotation.z = q.z();
b2ir2_msg.transform.rotation.w = q.w();
_static_tf_broadcaster.sendTransform(b2ir2_msg);
_static_tf_broadcaster_->sendTransform(b2ir2_msg);

// Transform infra2 frame to infra1 optical frame
ir2_2_ir2o.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
Expand All @@ -929,7 +931,7 @@ class RealSenseCameraNode : public rclcpp::Node
ir2_2_ir2o_msg.transform.rotation.y = ir2_2_ir2o.getY();
ir2_2_ir2o_msg.transform.rotation.z = ir2_2_ir2o.getZ();
ir2_2_ir2o_msg.transform.rotation.w = ir2_2_ir2o.getW();
_static_tf_broadcaster.sendTransform(ir2_2_ir2o_msg);
_static_tf_broadcaster_->sendTransform(ir2_2_ir2o_msg);
}
}
// Publish Fisheye TF
Expand Down Expand Up @@ -1337,7 +1339,8 @@ class RealSenseCameraNode : public rclcpp::Node
std::map<stream_index_pair, int> _fps;
std::map<stream_index_pair, bool> _enable;
std::map<stream_index_pair, std::string> _stream_name;
tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster;

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _static_tf_broadcaster_;

std::map<stream_index_pair, image_transport::Publisher> _image_publishers;
std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr> _imu_publishers;
Expand All @@ -1358,6 +1361,8 @@ class RealSenseCameraNode : public rclcpp::Node
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo> _camera_info;
rclcpp::Publisher<realsense_camera_msgs::msg::Extrinsics>::SharedPtr _fe_to_depth_publisher,
_fe_to_imu_publisher;

rclcpp::QoS qos;
bool _intialize_time_base;
double _camera_time_base;
std::map<stream_index_pair, std::vector<rs2::stream_profile>> _enabled_profiles;
Expand Down

0 comments on commit 9cc4723

Please sign in to comment.