| 25 | using namespace std::chrono_literals; |
| 26 | |
| 27 | Odometry::Odometry( |
| 28 | std::shared_ptr<rclcpp::Node> & nh, |
| 29 | const double wheels_separation, |
| 30 | const double wheels_radius) |
| 31 | : nh_(nh), |
| 32 | wheels_separation_(wheels_separation), |
| 33 | wheels_radius_(wheels_radius), |
| 34 | use_imu_(false), |
| 35 | publish_tf_(false), |
| 36 | last_theta_initialized_(false), |
| 37 | imu_angle_(0.0f), |
| 38 | robot_pose_({0.0, 0.0, 0.0}), |
| 39 | robot_vel_({0.0, 0.0, 0.0}) |
| 40 | { |
| 41 | RCLCPP_INFO(nh_->get_logger(), "Init Odometry"); |
| 42 | |
| 43 | nh_->declare_parameter<std::string>("odometry.frame_id"); |
| 44 | nh_->declare_parameter<std::string>("odometry.child_frame_id"); |
| 45 | nh_->declare_parameter<std::string>("namespace"); |
| 46 | |
| 47 | nh_->declare_parameter<bool>("odometry.use_imu"); |
| 48 | nh_->declare_parameter<bool>("odometry.publish_tf"); |
| 49 | |
| 50 | nh_->get_parameter_or<bool>( |
| 51 | "odometry.use_imu", |
| 52 | use_imu_, |
| 53 | false); |
| 54 | |
| 55 | nh_->get_parameter_or<bool>( |
| 56 | "odometry.publish_tf", |
| 57 | publish_tf_, |
| 58 | false); |
| 59 | |
| 60 | nh_->get_parameter_or<std::string>( |
| 61 | "odometry.frame_id", |
| 62 | frame_id_of_odometry_, |
| 63 | std::string("odom")); |
| 64 | |
| 65 | nh_->get_parameter_or<std::string>( |
| 66 | "odometry.child_frame_id", |
| 67 | child_frame_id_of_odometry_, |
| 68 | std::string("base_footprint")); |
| 69 | |
| 70 | nh_->get_parameter_or<std::string>( |
| 71 | "namespace", |
| 72 | name_space_, |
| 73 | std::string("")); |
| 74 | |
| 75 | if (name_space_ != "") { |
| 76 | frame_id_of_odometry_ = name_space_ + "/" + frame_id_of_odometry_; |
| 77 | child_frame_id_of_odometry_ = name_space_ + "/" + child_frame_id_of_odometry_; |
| 78 | } |
| 79 | |
| 80 | auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); |
| 81 | odom_pub_ = nh_->create_publisher<nav_msgs::msg::Odometry>("odom", qos); |
| 82 | |
| 83 | tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(nh_); |
| 84 |
nothing calls this directly
no outgoing calls
no test coverage detected