nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
servo_tf_node.cpp
[詳解]
1#include <chrono>
2#include <cmath>
3#include <algorithm>
4#include <functional>
5#include <memory>
6#include <stdexcept>
7#include <string>
8#include <utility>
9
10#include "geometry_msgs/msg/transform_stamped.hpp"
11#include "rclcpp/rclcpp.hpp"
12#include "sensor_msgs/msg/joint_state.hpp"
13#include "tf2_ros/transform_broadcaster.h"
14
16
17class ServoTfNode : public rclcpp::Node
18{
19public:
21 : Node("servo_tf_node"),
22 last_state_time_(std::chrono::steady_clock::now())
23 {
24 state_topic_ = declare_parameter<std::string>("state_topic", "/servo_state");
25 joint_name_ = declare_parameter<std::string>("joint_name", "nodding_joint");
26 parent_frame_ = declare_parameter<std::string>("parent_frame", "servo_link");
27 child_frame_ = declare_parameter<std::string>("child_frame", "laser");
28 lidar_height_ = declare_parameter<double>("lidar_height", 0.0325);
29 state_timeout_sec_ = declare_parameter<double>("state_timeout_sec", 0.5);
30 if (state_timeout_sec_ <= 0.0) {
31 throw std::invalid_argument("state_timeout_sec must be positive");
32 }
33
34 broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
35 subscription_ = create_subscription<sensor_msgs::msg::JointState>(
36 state_topic_, rclcpp::QoS(10),
37 std::bind(&ServoTfNode::on_state, this, std::placeholders::_1));
38 watchdog_ = create_wall_timer(
39 std::chrono::milliseconds(500), std::bind(&ServoTfNode::check_state, this));
40 RCLCPP_INFO(
41 get_logger(), "Broadcasting %s -> %s from JointState '%s' (angle in radians)",
42 parent_frame_.c_str(), child_frame_.c_str(), state_topic_.c_str());
43 }
44
45private:
46 void on_state(const sensor_msgs::msg::JointState::ConstSharedPtr state)
47 {
48 const auto joint = std::find(state->name.begin(), state->name.end(), joint_name_);
49 if (joint == state->name.end()) {
50 RCLCPP_WARN_THROTTLE(
51 get_logger(), *get_clock(), 2000, "JointState does not contain joint '%s'",
52 joint_name_.c_str());
53 return;
54 }
55 const auto index = static_cast<size_t>(std::distance(state->name.begin(), joint));
56 if (index >= state->position.size() || !std::isfinite(state->position[index])) {
57 RCLCPP_WARN_THROTTLE(
58 get_logger(), *get_clock(), 2000, "JointState has no finite position for '%s'",
59 joint_name_.c_str());
60 return;
61 }
62 if (state->header.stamp.sec == 0 && state->header.stamp.nanosec == 0) {
63 RCLCPP_WARN_THROTTLE(
64 get_logger(), *get_clock(), 2000, "Ignoring servo angle with a zero timestamp");
65 return;
66 }
67
68 const nodding_lidar::Pose pose =
69 nodding_lidar::servo_to_lidar_pose(state->position[index], lidar_height_);
70 geometry_msgs::msg::TransformStamped transform;
71 transform.header = state->header;
72 transform.header.frame_id = parent_frame_;
73 transform.child_frame_id = child_frame_;
74 transform.transform.translation.x = pose.t.x;
75 transform.transform.translation.y = pose.t.y;
76 transform.transform.translation.z = pose.t.z;
77 transform.transform.rotation.x = pose.qx;
78 transform.transform.rotation.y = pose.qy;
79 transform.transform.rotation.z = pose.qz;
80 transform.transform.rotation.w = pose.qw;
81 broadcaster_->sendTransform(transform);
82
83 last_state_time_ = std::chrono::steady_clock::now();
84 have_state_ = true;
85 }
86
87 void check_state()
88 {
89 const auto elapsed = std::chrono::duration<double>(
90 std::chrono::steady_clock::now() - last_state_time_).count();
91 if (!have_state_ || elapsed > state_timeout_sec_) {
92 RCLCPP_WARN_THROTTLE(
93 get_logger(), *get_clock(), 2000,
94 "No valid servo angle received recently; dynamic TF updates have stopped");
95 }
96 }
97
98 std::string state_topic_;
99 std::string joint_name_;
100 std::string parent_frame_;
101 std::string child_frame_;
102 double lidar_height_{0.0325};
103 double state_timeout_sec_{0.5};
104 bool have_state_{false};
105 std::chrono::steady_clock::time_point last_state_time_;
106 rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
107 rclcpp::TimerBase::SharedPtr watchdog_;
108 std::unique_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
109};
110
111int main(int argc, char ** argv)
112{
113 rclcpp::init(argc, argv);
114 int result = 0;
115 try {
116 rclcpp::spin(std::make_shared<ServoTfNode>());
117 } catch (const std::exception & e) {
118 RCLCPP_FATAL(rclcpp::get_logger("servo_tf_node"), "%s", e.what());
119 result = 1;
120 }
121 rclcpp::shutdown();
122 return result;
123}
首振り角度と取付寸法を使い、LiDAR点を別の座標系へ変換する。
Pose servo_to_lidar_pose(double phi, double lidar_height)
int main(int argc, char **argv)